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-12-15 12:50:30 +08:00
parent e2b60094b3
commit 45a9b18a96
11 changed files with 188 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
COM/COM_Robot.py
View File

@ -37,7 +37,7 @@ class RobotClient(TCPClient):
self.max_angle_interval = 0
def add_sendQuene(self,command): #后面 命令分等级，紧急命令直接执行
self.command_quene.put(command)
log.log_message(logging.INFO, f'{Constant.str_sys_command}{command}')
return
def send_Command(self):

3
CU/Catch.py
View File

@ -41,11 +41,12 @@ class Catch:
else:
self.shake_continue.SetReset()
self.catch_status = CatchStatus.COk
if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
#if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
self.robotClient.sendIOControl(self.robotClient.con_ios[2], 0)
print("震动结束")
if self.catch_status == CatchStatus.COk:
self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
pass

33
CU/Feeding.py
View File

@ -206,11 +206,11 @@ class Feeding(QObject):
# 0,
# 0);
img_path = f'Image/{self.index}.png'
img = cv2.imread(img_path)
self.index += 1
self.index = self.index % 4
self.detection_image = img
# img_path = f'Image/{self.index}.png'
# img = cv2.imread(img_path)
# self.index += 1
# self.index = self.index % 4
# self.detection_image = img
if self.feedConfig == None:
self.feedStatus = FeedStatus.FNone
@ -247,6 +247,7 @@ class Feeding(QObject):
log.log_message(logging.INFO, Constant.str_feed_start)
if not self.robotClient.origin_position.compare(real_position) and not self.is_reverse:
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
log.log_message(logging.ERROR, Constant.str_feed_start_error)
self.need_origin_signal.emit(Constant.str_feed_start_error)
self.feedStatus = FeedStatus.FNone
return
@ -260,7 +261,7 @@ class Feeding(QObject):
# = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,
# # save_img_point=0, Height_reduce=30, width_reduce=30)
if self.feedConfig.feedLine.get_take_position().get_position() != None:
self.feedConfig.feedLine.set_take_position(self.get_take_position())
# TODO 获取目标位置
#
@ -368,8 +369,7 @@ class Feeding(QObject):
# self.sendIOControl(self.robotClient.con_ios[1], 1)
# self.sendIOControl(self.robotClient.con_ios[2], 1)
log.log_message(logging.INFO, "到达抓料点位")
if self.catch.catch_status == CatchStatus.CNone :
self.catch.catch_status = CatchStatus.CTake
return
@ -492,7 +492,6 @@ class Feeding(QObject):
return
pos_model = self.reversed_positions[self.reverse_index]
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
print('跳过抓袋点位')
pos_model = self.reversed_positions[self.reverse_index + 1]
self.reverse_index = self.reverse_index+1
@ -560,10 +559,10 @@ class Feeding(QObject):
while self.run_reverse and self.reverse_index!=len(reversed_positions):
if self.reverse_index!=0 and not real_position.compare(current_position.get_position()):
continue
print(f'复位第{self.reverse_index}') #todo 缺少比对
#todo 缺少比对
pos_model = reversed_positions[self.reverse_index]
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
print('跳过抓袋点位')
pos_model = reversed_positions[self.reverse_index + 1]
# TODO take节点判断
@ -627,12 +626,12 @@ class Feeding(QObject):
f'U:{position_instruction.m3}-' \
f'V:{position_instruction.m4}-' \
f'W:{position_instruction.m5}'
print(log_str)
try:
log.log_message(logging.INFO, log_str)
except:
print("error")
return
self.robotClient.add_sendQuene(request_command)
pass
@ -640,16 +639,15 @@ class Feeding(QObject):
if Constant.Debug:
return self.robotClient.status_model.getRealPosition()
_, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,save_img_point=0, Height_reduce=30, width_reduce=30)
self.detection_image = img
self.detection_image = img.copy()
if xyz ==None or uvw==None or points==None:
return None
target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
position.Z = position.Z+40
position.Z = position.Z+200
return position
def next_start(self,reverse=False):
print(f'原点到初始位置的第{self.feedConfig.feedLine.origin2start_pos_index}')
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
if start_pos.lineType == LineType.CureMid.value:
@ -667,7 +665,7 @@ class Feeding(QObject):
pass
def next_take(self,reverse=False):
print(f'初始位置到拍照点的第{self.feedConfig.feedLine.start2take_pos_index}')
take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
if take_pos.lineType == LineType.CureMid.value:
@ -679,7 +677,6 @@ class Feeding(QObject):
pass
def next_Feed(self,reverse=False):
print(f'拍照到抛料回来的第{self.feedConfig.feedLine.feeding2end_pos_index}')
feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
if feed_pos.lineType == LineType.CureMid.value:

228
Config/FeedLine.ini
View File

@ -2,163 +2,163 @@
id = 1
name = 反应釜1
[Position2]
x = 2.734364
y = -0.014967
z = -0.353024
u = -19.845549
v = -21.947229
w = 2.585297
id = 2
order = 0
lineid = 1
status = 3
linetype = 4
[FeedLine2]
id = 2
name = 未定义
name = 反应釜2
[Position3]
x = 1.0
y = 2.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
x = 1882.882568
y = 786.492737
z = 1203.552246
u = 11.403661
v = -0.985981
w = -125.710434
id = 3
order = 0
lineid = 2
status = 3
linetype = 0
[Position5]
x = 693.6604
y = -7.213258
z = 728.627869
u = -156.791855
v = 64.281898
w = -157.13649
id = 5
order = 3
[Position4]
x = 1868.06311
y = 872.854065
z = 1265.651855
u = 12.479004
v = 1.611398
w = -127.729263
id = 4
order = 2
lineid = 1
status = 4
linetype = 0
[Position4]
x = 693.677979
y = -24.01004
z = 730.956055
u = -141.801926
v = 60.324791
w = -143.841553
id = 4
order = 4
lineid = 1
status = 3
linetype = 0
[Position1]
x = 693.6604
y = -7.213258
z = 728.627869
u = -156.791855
v = 64.281898
w = -157.13649
x = 348.84654634488436
y = 1836.958560527403
z = 223.84778176988266
u = 2.3775411141593388
v = -0.7172354339862681
w = 95.0492701673298
id = 1
order = 5
order = 4
lineid = 1
status = 5
linetype = 0
[Position6]
x = 693.677979
y = -24.01004
z = 730.956055
u = -141.801926
v = 60.324791
w = -143.841553
id = 6
order = 2
[Position7]
x = 2218.886475
y = 483.322144
z = 1371.784302
u = 8.190439
v = 7.692511
w = -143.628922
id = 7
order = 0
lineid = 1
status = 2
status = 3
linetype = 0
[Position7]
x = 693.677979
y = -24.01004
z = 730.956055
u = -141.801926
v = 60.324791
w = -143.841553
id = 7
order = 6
[Position8]
x = 807.391785
y = 2337.584473
z = 1852.736816
u = -0.412497
v = 9.080836
w = -110.687553
id = 8
order = 5
lineid = 1
status = 6
linetype = 0
[Position8]
x = 693.6604
y = -7.213258
z = 728.627869
u = -156.791855
v = 64.281898
w = -157.13649
id = 8
order = 7
[Position9]
x = 1759.061523
y = 1738.14978
z = 1852.472412
u = 0.723375
v = 9.708878
w = -126.572548
id = 9
order = 6
lineid = 1
status = 7
linetype = 0
[Position9]
x = 693.677979
y = -24.01004
z = 730.956055
u = -141.801926
v = 60.324791
w = -143.841553
id = 9
order = 8
[Position10]
x = 1759.061523
y = 1738.14978
z = 1852.472412
u = 0.723375
v = 9.708878
w = -126.572548
id = 10
order = 7
lineid = 1
status = 8
linetype = 0
[Position10]
x = 693.6604
y = -7.213258
z = 728.627869
u = -156.791855
v = 64.281898
w = -157.13649
id = 10
order = 9
lineid = 1
status = 9
linetype = 0
[Position11]
x = 693.677979
y = -24.01004
z = 730.956055
u = -141.801926
v = 60.324791
w = -143.841553
x = 1960.667847
y = 837.290405
z = 1688.202637
u = -0.347183
v = 0.356827
w = -147.846161
id = 11
order = 10
order = 8
lineid = 1
status = 3
linetype = 0
[Position12]
x = 2.719969
y = 0.0
z = -0.338814
u = -34.504669
v = -21.932972
w = 2.570882
x = 1643.540039
y = -24.557989
z = 995.504272
u = 0.167181
v = 2.42627
w = 177.088989
id = 12
order = 1
order = 9
lineid = 1
status = 9
linetype = 0
[Position2]
x = 762.708984
y = 1915.674561
z = 1265.639648
u = 12.478849
v = 1.611004
w = -84.483177
id = 2
order = 3
lineid = 1
status = 3
linetype = 4
linetype = 0
[Position5]
x = 1868.06311
y = 872.854065
z = 1265.651855
u = 12.479004
v = 1.611398
w = -127.729263
id = 5
order = 1
lineid = 1
status = 2
linetype = 0
[Position6]
x = 1425.824829
y = 952.627869
z = 1364.459839
u = -5.591513
v = -38.629269
w = -114.406639
id = 6
order = 1
lineid = 2
status = 2
linetype = 0

5
Constant.py
View File

@ -1,9 +1,9 @@
import os
Debug = True
Debug = False
IO_EmergencyPoint = 3
bag_height = 10 # 一袋的高度
position_accuracy = 0.05
position_accuracy = 0.1
manual_adjust_accuracy = 1
# speed = 10
# shake_speed = 20
@ -78,6 +78,7 @@ str_sys_log_IO_error = 'IO更新失败'
str_sys_log_alarm_error = '发生报警：'
str_sys_log_move_error = '请填写全部坐标'
str_sys_set_position_error = '未选中行'
str_sys_command = '发送命令'
str_tcp_robot_connect_fail = '连接失败'
str_tcp_robot_connect_success = '连接成功'
str_tcp_robot_data_error = '数据解析错误'

29
Model/Position.py
View File

@ -1,3 +1,5 @@
import math
from Constant import position_accuracy
class Position:
def __init__(self):
@ -7,17 +9,30 @@ class Position:
self.U = 0.0
self.V = 0.0
self.W = 0.0
def compare(self,position):
if self.X-position.X<position_accuracy and \
self.Y-position.Y<position_accuracy and \
self.Z - position.Z < position_accuracy and \
self.U - position.U < position_accuracy and \
self.V - position.V < position_accuracy and \
self.W - position.W < position_accuracy:
distance = math.sqrt((self.X-position.X)**2+
(self.Y-position.Y)**2+
(self.Z - position.Z)**2+
(self.U - position.U)**2+
(self.V - position.V)**2+
(self.W - position.W) ** 2)
if distance<=position_accuracy:
return True
else:
return False
pass
# def compare(self,position):
# if self.X-position.X<position_accuracy and \
# self.Y-position.Y<position_accuracy and \
# self.Z - position.Z < position_accuracy and \
# self.U - position.U < position_accuracy and \
# self.V - position.V < position_accuracy and \
# self.W - position.W < position_accuracy:
# return True
# else:
# return False
# pass
def is_error_angel_move(self,position,interval):
if self.X - position.X >= interval or \

3
Model/RobotModel.py
View File

@ -189,8 +189,7 @@ class CMDInstructRequest:
if len(self.instructions) != 0:
model_str = model_str+',"instructions":'+"[{"+self.instructions[0].toString()+"}]"+"}"
else:
model_str = model_str+',"instructions":'+"[{""}]"+"}" #model_str+"}"
print(model_str)
model_str = model_str+',"instructions":'+"[]"+"}" #model_str+"}"
return model_str
class CMDInstructReply:

22
Seting.ini
View File

@ -1,7 +1,7 @@
[Main]
[Robot_Feed]
ipaddress = 127.0.0.1
ipaddress = 192.168.20.4
port = 502
j1_min = -150
j1_max = +150
@ -48,13 +48,13 @@ photo_w5 = 1.0
linecount = 2
remain_linename = 1
remain_count = 0
io_take_addr = 3
io_zip_addr = 2
io_shake_addr = 10
io_take_addr = 8
io_zip_addr = 11
io_shake_addr = 12
takedelay = 0.2
putdelay = 0.1
shakedelay = 1.0
max_angle_interval=20
max_angle_interval = 20
[Speed]
debug_speed = 50
@ -62,12 +62,12 @@ feed_speed = 10
reset_speed = 35
[Origin]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
x = 1927.155273
y = 570.19989
z = 1299.689941
u = 6.975893
v = -9.897896
w = -135.095978
[Camera_Feed]
ipaddress = 127.0.0.1

6
Util/util_pic.py
View File

@ -11,11 +11,13 @@ def cv2_to_qpixmap(cv_img):
# img = cv_img.copy()
# cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2RGB)
try:
height, width, channel = cv_img.shape
img = cv_img.copy()
height, width, channel = img.shape
bytes_per_line = 3 * width
q_img = QImage(cv_img.data, width, height, bytes_per_line, QImage.Format_RGB888)
q_img = QImage(img.data, width, height, bytes_per_line, QImage.Format_RGB888)
return QPixmap.fromImage(q_img)
except Exception as e:
print(e)
log.log_message(logging.ERROR,e)
return None

3
app.py
View File

@ -910,7 +910,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
else:
pause_command.cmdData.append("0")
request_command = pause_command.toString()
print(request_command)
self.robotClient.add_sendQuene(request_command)
def send_clear_alarm_command(self, pause: bool):
@ -918,7 +917,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
pause_command.cmdData.append("clearAlarmContinue")
pause_command.cmdData.append("1")
request_command = pause_command.toString()
print(request_command)
log_str = f'暂停：{pause}'
self.robotClient.add_sendQuene(request_command)
@ -927,7 +925,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
switch_command.cmdData.append("switchTool")
switch_command.cmdData.append("2")
request_command = switch_command.toString()
print(request_command)
self.robotClient.add_sendQuene(request_command)

30
main.py
View File

@ -397,8 +397,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
continue
position_model = PositionModel(i)
break
if position_model.id == 999:
print("d")
if self.tableWidget_line_positions.currentRow()==-1:
row_i = self.tableWidget_line_positions.rowCount()
else:
@ -814,12 +812,13 @@ class MainWindow(QMainWindow, Ui_MainWindow):
def start_Runing(self):
self.main_threading = Thread(target=self.run)
self.robot_connect_threading = Thread(target=self.robotClient.run)
self.main_UI_threading = Thread(target=self.updateUI)
self.detect_person_thread = Thread(target=self.run_detect_persion)
self.main_threading.start()
self.robot_connect_threading.start()
self.main_UI_threading = Thread(target=self.updateUI)
self.main_UI_threading.start()
self.detect_person_thread = Thread(target=self.run_detect_persion)
self.detect_person_thread.start()
pass
def check_continue(self):
@ -1207,8 +1206,8 @@ class MainWindow(QMainWindow, Ui_MainWindow):
log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
return
self.feeding.reset_status = ResetStatus.RStart
dialog_reset = StopDialog()
dialog_reset.stop_thread_signal.connect(self.stop_reset_thread)
# dialog_reset = StopDialog()
# dialog_reset.stop_thread_signal.connect(self.stop_reset_thread)
else:
log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
return
@ -1564,7 +1563,8 @@ class MainWindow(QMainWindow, Ui_MainWindow):
clear_command = CMDInstructRequest()
request_command = clear_command.toString()
log.log_message(logging.INFO, Constant.str_sys_clearAlarm)
self.command_quene.put(request_command)
self.robotClient.add_sendQuene(request_command)
# self.command_quene.put(request_command)
def send_position_command(self, x1, x2, x3, x4, x5, x6, move_type: MoveType = MoveType.WORLD):
@ -1598,12 +1598,11 @@ class MainWindow(QMainWindow, Ui_MainWindow):
else:
pause_command.cmdData.append("0")
request_command = pause_command.toString()
print(request_command)
self.robotClient.add_sendQuene(request_command)
def send_emergency_alarm_command(self):
self.feeding.feedStatus = FeedStatus.FNone
self.feeding.feedConfig.num = 0
stop_command = CMDRequest()
stop_command.cmdData.append("actionStop")
stop_command.cmdData.append("1")
@ -1611,18 +1610,17 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.send_clear_auto_command()
self.robotClient.add_sendQuene(request_command)
self.feeding.send_emergency_sound()
self.feeding.feedConfig.num = 0
log.log_message(logging.INFO, Constant.str_sys_emergencyStop)
#TODO
def send_clear_alarm_command(self, pause: bool):
self.send_start_tool_command()
self.send_clear_auto_command()
pause_command = CMDRequest()
pause_command.cmdData.append("StopButton")
pause_command.cmdData.append("1")
request_command = pause_command.toString()
print(request_command)
log_str = f'暂停：{pause}'
self.robotClient.add_sendQuene(request_command)
def send_switch_tool_command(self):
@ -1630,8 +1628,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
switch_command.cmdData.append("switchTool")
switch_command.cmdData.append("2")
request_command = switch_command.toString()
print(request_command)
self.robotClient.add_sendQuene(request_command)
def send_start_tool_command(self):
@ -1639,8 +1635,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
switch_command.cmdData.append("startButton")
switch_command.cmdData.append("1")
request_command = switch_command.toString()
print(request_command)
self.robotClient.add_sendQuene(request_command)
def show_messagebox_of_person(self):
@ -1845,6 +1839,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.configReader.write(open(Constant.set_ini, 'w', encoding='utf-8'))
log.log_message(logging.INFO, f'设置原点:{x},{y},{z}')
except:
log.log_message(logging.ERROR, f'设置原点失败')
self.show_infomessage_box("设置原点失败")
@ -1970,7 +1965,6 @@ def handle_exception(exc_type, exc_value, exc_tb):
log.log_message(logging.INFO, "用户主动退出程序")
return
log.log_message(logging.ERROR, exc_value)
print("未处理的异常:", exc_type, exc_value)
traceback.print_exception(exc_type, exc_value, exc_tb)
if __name__ == "__main__":