diff --git a/COM/COM_Robot.py b/COM/COM_Robot.py index a9f3989..fcad7d1 100644 --- a/COM/COM_Robot.py +++ b/COM/COM_Robot.py @@ -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): diff --git a/CU/Catch.py b/CU/Catch.py index 777f071..9421749 100644 --- a/CU/Catch.py +++ b/CU/Catch.py @@ -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]): - self.robotClient.sendIOControl(self.robotClient.con_ios[2], 0) - print("震动结束") + #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 diff --git a/CU/Feeding.py b/CU/Feeding.py index fb24055..1ccd29f 100644 --- a/CU/Feeding.py +++ b/CU/Feeding.py @@ -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,8 +261,8 @@ 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) - - self.feedConfig.feedLine.set_take_position(self.get_take_position()) + if self.feedConfig.feedLine.get_take_position().get_position() != None: + self.feedConfig.feedLine.set_take_position(self.get_take_position()) # TODO 获取目标位置 # self.feed_Mid_Status = FeedMidStatus.FMid_Start @@ -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: diff --git a/Config/FeedLine.ini b/Config/FeedLine.ini index c1b1bc5..baf99b6 100644 --- a/Config/FeedLine.ini +++ b/Config/FeedLine.ini @@ -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 diff --git a/Constant.py b/Constant.py index 441d276..a4a2c19 100644 --- a/Constant.py +++ b/Constant.py @@ -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 = '数据解析错误' diff --git a/Model/Position.py b/Model/Position.py index 9c8824b..0ffdb8a 100644 --- a/Model/Position.py +++ b/Model/Position.py @@ -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= interval or \ diff --git a/Model/RobotModel.py b/Model/RobotModel.py index 9298e56..6a82a53 100644 --- a/Model/RobotModel.py +++ b/Model/RobotModel.py @@ -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: diff --git a/Seting.ini b/Seting.ini index 8eb6424..cdbad51 100644 --- a/Seting.ini +++ b/Seting.ini @@ -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 diff --git a/Util/util_pic.py b/Util/util_pic.py index e100827..4b11dd9 100644 --- a/Util/util_pic.py +++ b/Util/util_pic.py @@ -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 diff --git a/app.py b/app.py index 579de31..f33e28d 100644 --- a/app.py +++ b/app.py @@ -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) diff --git a/main.py b/main.py index 1328ba7..3553c9f 100644 --- a/main.py +++ b/main.py @@ -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__":