xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update 初次融合+调试

Browse Source
This commit is contained in:
FrankCV2048
2024-09-03 23:28:16 +08:00
parent 051de571a8
commit 0af995b9cd
10 changed files with 108 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
COM/COM_Robot.py
View File

@ -1,11 +1,11 @@
from COM.COM_TCP import TCPClient
import queue
import json
from Model.RobotModel import DataAddress,DATARequest
from Model.RobotModel import DataAddress,DATARequest,DATAReply
class RobotClient(TCPClient):
def __init__(self, ip, port, command_quene, status_model):
def __init__(self, ip, port, command_quene, status_model:DataAddress):
super().__init__(ip, port)
self.command_quene = command_quene
self.status_model = status_model
@ -16,26 +16,27 @@ class RobotClient(TCPClient):
return
def send_Command(self):
q = queue.Queue
q.qsize()
while True: # 让出时间间隔
try:
if self.command_quene.qsize()!=0:
command = self.command_quene.get()
self.client_socket.send(json.dumps(command).encode('utf-8'))
try:
if self.command_quene.qsize()!=0:
command = self.command_quene.get()
self.client_socket.send(command.encode('utf-8'))
if False:
response = self.client_socket.recv(1024).decode('utf-8')
response_message = json.loads(response)
if True:
print('正确相应')
else:
self.errorCommands[json.dumps(command).encode('utf-8')] = response_message
print('出错')
return True
else:
return True
except Exception as e:
print('连接断开')
return False
if False:
response = self.client_socket.recv(1024).decode('utf-8')
response_message = json.loads(response)
if True:
print('正确相应')
else:
self.errorCommands[json.dumps(command).encode('utf-8')] = response_message
print('出错')
except Exception as e:
print('连接断开')
#return False
def send_Status(self):
request = DATARequest()
@ -45,25 +46,23 @@ class RobotClient(TCPClient):
# 移除特殊属性和方法
request_status_json = request.toString()
# 转字符串
while True:
try:
self.client_socket.send(json.dumps(request_status_json).encode('utf-8'))
if False:
response = self.client_socket.recv(1024).decode('utf-8')
response_message = json.loads(response)
if True:
data_status = DATARequest()
data_status.__dict__ = response_message
data_address_array = data_status.queryAddr
data_Address = DataAddress()
for i in attributes.count():
setattr(data_Address,attributes[i],data_address_array[i])
self.status_model = data_Address
else:
print('转换失败')
except Exception as e:
print('连接断开')
try:
self.client_socket.send(request_status_json.encode('utf-8'))
if True:
response = self.client_socket.recv(1024).decode('utf-8')
response_message = json.loads(response)
if True:
data_status = DATAReply()
data_status.__dict__ = response_message
data_address_array = data_status.queryAddr
data_Address = DataAddress()
data_Address.setPosition(*data_address_array)
self.status_model.setPosition(*data_address_array)
return True
except Exception as e:
return False
return False

3
COM/COM_TCP.py
View File

@ -30,10 +30,9 @@ class TCPClient:
time.sleep(0.2)
self.connected = self.error_count > 0
try:
#time.sleep(30)
if (self.send_Status() and self.send_Command()):
self.error_count = 0
except:
except Exception as e:
self.error_count += 1

25
CU/Feeding.py
View File

@ -1,7 +1,8 @@
from Model.Position import Real_Position
from Model.Position import Real_Position, Detection_Position
from enum import Enum
from COM.COM_Robot import RobotClient
from Model.RobotModel import CMDInstructRequest
from Vision.camera_coordinate_dete import Detection
class FeedStatus(Enum):
@ -33,7 +34,7 @@ class FeedingConfig:
class Feeding():
def __init__(self,robotClient:RobotClient):
def __init__(self,robotClient:RobotClient,detection: Detection):
self.feedConfig = None
self.feedStatus = FeedStatus.FNone
self.robotClient = robotClient
@ -75,8 +76,24 @@ class Feeding():
elif self.feedStatus == FeedStatus.FPhoto:
if self.feedConfig.photo_position.compare(real_position):
self.feedStatus = FeedStatus.FTake
pass # 发送拍照获取坐标 并 开始移动
_, _, xyz, uvw = self.detection.get_position() #检测结果
if xyz!=None:
self.feedStatus = FeedStatus.FTake
#dp = Detection_Position().init_position(*xyz, *uvw)
from Trace.handeye_calibration import R_matrix,getPosition
rotation = R_matrix(self.robotClient.status_model.world_0,
self.robotClient.status_model.world_1,
self.robotClient.status_model.world_2,
self.robotClient.status_model.world_3,
self.robotClient.status_model.world_4,
self.robotClient.status_model.world_5)
# 黄老师给我的xyz和法向量
target_position, noraml_base = getPosition(*xyz, *uvw,rotation)
self.feedConfig.take_position = Real_Position().init_position(*target_position,*noraml_base)
else:
print("打印日志，保存失败图像")
pass # 发送拍照获取坐标 并 开始移动
elif self.feedStatus == FeedStatus.FTake:
if self.feedConfig.take_position != None and self.feedConfig.take_position.compare(real_position):

1
Constant.py Normal file
View File

@ -0,0 +1 @@
position_accuracy=0.02

2
Model/Position.py
View File

@ -1,4 +1,4 @@
from Constant import position_accuracy
class Position:
def __init__(self):
self.X = 0.0

30
Model/RobotModel.py
View File

@ -5,7 +5,7 @@ class DATARequest:
self.queryAddr = []
def toString(self):
model_str = '{'+f'"dsID":"{self.dsID}","reqType":"{self.reqType}","queryAddr":[' \
f'"{self.queryAddr[0].toString()}"]'+'}'
f'{self.queryAddr[0].toString()}]'+'}'
return model_str
@ -22,7 +22,12 @@ class DataAddress:
self.output_n = ''
self.axisNum = '6'
self.axis_n = ''
self.world_n = ''
self.world_0 = 0
self.world_1 = 0
self.world_2 = 0
self.world_3 = 0
self.world_4 = 0
self.world_5 = 0
self.curAlarm = ''
self.curCycle = ''
self.lastCycle = ''
@ -35,9 +40,17 @@ class DataAddress:
self.isMoving = ''
self.M_n = ''
#return
def setPosition(self,w0,w1,w2,w3,w4,w5):
self.world_0 = int(w0)
self.world_1 = int(w1)
self.world_2 = int(w2)
self.world_3 = int(w3)
self.world_4 = int(w4)
self.world_5 = int(w5)
def toString(self):
model_str = f'"world_n"'
model_str = f'"world_0","world_1","world_2","world_3","world_4","world_5"'
return model_str
@ -48,8 +61,8 @@ class DATAReply:
def __init__(self):
self.dsID = ''
self.reqType = ''
self.queryData = DataAddress()
return
self.queryData = []
def JsonToObject(self):
@ -72,7 +85,7 @@ class CMDReply:
class Instruction:
def __init__(self):
self.oneshot = 1
self.action = 4 #4 自由路径 10 姿势直线 17 姿势曲线
self.action = 10 #4 自由路径 10 姿势直线 17 姿势曲线
self.m0 = 0.0
self.m1 = 0.0
self.m2 = 0.0
@ -80,13 +93,14 @@ class Instruction:
self.m4 = 0.0
self.m5 = 0.0
self.ckStatus = '0x3F'
self.speed =10
self.speed = 10
self.delay = 1.0
self.smooth = 0
def toString(self):
model_str = f'"oneshot":"{self.oneshot}","action":"{self.action}","m0":"{self.m0}","m1":"{self.m1}","m2":"{self.m2}",' \
f'"m3":"{self.m3}","m4":"{self.m4}","m5":"{self.m5}"'
f'"m3":"{self.m3}","m4":"{self.m4}","m5":"{self.m5}","ckStatus":"{self.ckStatus}","speed":"{self.speed}",' \
f'"delay":"{self.delay}","smooth":"{self.smooth}"'
return model_str

4
Seting.ini
View File

@ -2,8 +2,8 @@
[Robot_Feed]
IPAddress=192.168.10.4
Port=502
IPAddress=192.168.3.5
Port=8116
j1_min=-150
j1_max=+150
j2_min=-150

10
Trace/handeye_calibration.py
View File

@ -58,12 +58,11 @@ def R_matrix(x,y,z,u,v,w):
return transformation_matrix
rotation = R_matrix(559.365,0.704,731.196,179.996,66.369,179.996)#张啸给我的值填这里
# 黄老师给我的xyz和法向量
def getPosition(x,y,z,a,b,c):
# 图像识别结果：xyz和法向量
def getPosition(x,y,z,a,b,c,rotation):
target = np.asarray([x, y, z,1])
camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ')
camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ') #相对目录且分隔符采用os.sep
robot2base = rotation
camera2base = robot2base @ camera2robot
target_position = np.dot(camera2base, target)
@ -75,6 +74,3 @@ def getPosition(x,y,z,a,b,c):
print(target_position, noraml_base)
return target_position,noraml_base
if __name__ =="__main__":
getPosition(-96.09919,56.339145,444.640084,-0.09619,-0.45399, 0.88579)

2
Vision/camera_coordinate_dete.py
View File

@ -15,7 +15,7 @@ from .tool.CameraRVC import camera
from .yolo.yolov8_pt_seg import yolov8_segment
class Detection_Position:
class Detection:
def __init__(self, model_path, device):
self.camera_rvc = camera()

23
app.py
View File

@ -9,6 +9,7 @@ from PySide6.QtWidgets import QApplication, QMainWindow, QPushButton
from datetime import datetime
from CU.Command import FeedCommand
from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus
from Vision.camera_coordinate_dete import Detection
from ui_untitled import Ui_MainWindow
from COM.COM_Robot import RobotClient
from Expection import Error_Code
@ -16,7 +17,7 @@ from queue import Queue
from Model.RobotModel import *
import time
from queue import Queue
from Model.Position import Real_Position
from Model.Position import Real_Position, Detection_Position
from threading import Thread
from CU.Command import Status
@ -64,7 +65,7 @@ class MainWindow(QMainWindow,Ui_MainWindow):
def init_Run(self):
self.robotClient = None
self.configReader = configparser.ConfigParser()
self.detection = Detection()
self.command_position_quene = Queue()
self.status_address = DataAddress()
self.feedLine_dict = {}
@ -75,7 +76,7 @@ class MainWindow(QMainWindow,Ui_MainWindow):
ip = self.configReader.get('Robot_Feed', 'IPAddress')
port = int(self.configReader.get('Robot_Feed', 'Port'))
self.robotClient = RobotClient(ip, port, self.command_position_quene, self.status_address)
self.feeding = Feeding(self.robotClient) # 临时
self.feeding = Feeding(self.robotClient,self.detection) # 临时
self.last_time=time.time()
try:
self.robotClient.CreatConnect()
@ -257,8 +258,8 @@ class MainWindow(QMainWindow,Ui_MainWindow):
self.horizontalSlider_J6.setValue(int(self.lineEdit_j6.text()))
instruction_command = CMDInstructRequest()
instruction_command.instructions.append(vars(position_instruction))
request_command = vars(instruction_command)
instruction_command.instructions.append(position_instruction)
request_command = instruction_command.toString()
print(request_command)
self.robotClient.add_sendQuene(request_command)
@ -301,6 +302,18 @@ class MainWindow(QMainWindow,Ui_MainWindow):
self.label_date.setText(datetime.now().strftime("%Y-%m-%d"))
self.label_time.setText(datetime.now().strftime("%H:%M:%S"))
self.updateUI_Position()
def updateUI_Position(self):
self.horizontalSlider_J1.setValue(self.status_address.world_0)
self.horizontalSlider_J2.setValue(self.status_address.world_1)
self.horizontalSlider_J3.setValue(self.status_address.world_2)
self.horizontalSlider_J4.setValue(self.status_address.world_3)
self.horizontalSlider_J5.setValue(self.status_address.world_4)
self.horizontalSlider_J6.setValue(self.status_address.world_5)
def set_label_status_style(self, connected: bool):
if connected: