update 完成所有投料逻辑，IO点位控制，急停控制

COM/COM_Robot.py

@@ -9,12 +9,13 @@ from Util.util_log import log
 
 class RobotClient(TCPClient):
 
-    def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress):
+    def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress,con_ios):
         super().__init__(ip, port)
         self.command_quene = command_quene
         self.status_model = status_model
         self.errorCommands = {}
         self.photo_locs = photo_locs
+        self.con_ios = con_ios
     def add_sendQuene(self,command): #后面 命令分等级，紧急命令直接执行
         self.command_quene.put(command)
         return

CU/Feeding.py

@@ -42,8 +42,12 @@ class FeedingConfig:
     def __init__(self, num:int, feedLine:FeedLine,photo_locs):
         self.num = num
         self.feedLine = feedLine
-        self.photo_locs = photo_locs
+        self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
 
+    def deal_photo_locs(self,photo_loc):
+        position_photo = Real_Position()
+        position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5])
+        return position_photo
 
     def get_line_info(self):
         pass
@@ -93,9 +97,9 @@ class Feeding:
             log.log_message(logging.INFO, Constant.str_feed_check)
             # 1, 检查是否是三列
             # 2, 检查是否有人
-            if self.safe_check_columns() and self.safe_check_person():
+            # if self.safe_check_columns() and self.safe_check_person():
-                pass
+            #     pass
-            else:
+            # else:
             if self.feedConfig.num != 0:
                 self.feedStatus = FeedStatus.FSafeP
                 self.sendTargPosition(self.feedConfig.feedLine.safe_position)
@@ -105,14 +109,17 @@ class Feeding:
              log.log_message(logging.INFO,Constant.str_feed_safe)
              if self.feedConfig.feedLine.safe_position.compare(real_position):
                 self.feedStatus = FeedStatus.FPhoto
-                self.sendTargPosition(self.feedConfig.feedLine.photo_position)
+                #self.sendTargPosition(self.feedConfig.feedLine.photo_position)
              #判断是哪一个问题
 
         elif self.feedStatus == FeedStatus.FPhoto:
             log.log_message(logging.INFO, Constant.str_feed_photo)
-            if self.feedConfig.feedLine.photo_position.compare(real_position):
+            detect_pos_list = []
+            for pos in self.feedConfig.photo_locs:
+                self.sendTargPosition(pos)
+                while not pos.compare(real_position): #可以优化 TODO
+                    time.sleep(0.1)
                 code, img, xyz, uvw, mng = self.detection.get_position() #检测结果
-                log.log_message(logging.INFO, Constant.str_feed_takePhoto)
                 self.detection_image = img
                 if xyz != None:
                     log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
@@ -120,26 +127,49 @@ class Feeding:
                     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-10,
+                                        self.robotClient.status_model.world_2 - 10, #TODO 这个10 需要确定
                                         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, *mng)
-                    log.log_message(logging.INFO, Constant.str_feed_covert_success)
+                    detect_pos_list.append(Real_Position().init_position(*target_position[:3],*noraml_base))
-                    self.feedConfig.feedLine.take_position = Real_Position().init_position(*target_position[:3],*noraml_base)
+                    log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
+                else:
+                    log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail+real_position.to_string())
+            z_diff, max_z_index = (lambda pts: (
+                max(pts, key=lambda p: p.Z).Z - min(pts, key=lambda p: p.Z).Z,
+                pts.index(max(pts, key=lambda p: p.Z))
+            ))(detect_pos_list)
+            if len(self.feedConfig.photo_locs) == 5:
+                if z_diff < Constant.bag_height and len(detect_pos_list)==3: # 第一次检测到没有高度差距开始三列拍照 TODO 保留全部的开关
+                    # 拍照位置从五个变为三个
+                    self.feedConfig.photo_locs = [detect_pos_list[0],detect_pos_list[2],detect_pos_list[4]]
+                    self.feedConfig.feedLine.take_position = detect_pos_list[0]
+                    log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish)
+                else:
+                    self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
+                    log.log_message(logging.INFO, Constant.str_feed_takePhoto_front)
+            else:
+                if z_diff < Constant.bag_height:
+                    self.feedConfig.feedLine.take_position = detect_pos_list[0]
+                    log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line)
+                else:
+                    self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
+                    log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
             self.feedStatus = FeedStatus.FTake
             self.sendTargPosition(self.feedConfig.feedLine.take_position)
-                else:
+            log.log_message(logging.INFO, Constant.str_feed_takePhoto_move)
-                    log.log_message(logging.ERROR,Constant.str_feed_takePhoto_fail)
-                    print("打印日志，保存失败图像")
-
 
         elif self.feedStatus == FeedStatus.FTake:
             log.log_message(logging.INFO, Constant.str_feed_take)
             if self.feedConfig.feedLine.take_position != None and self.feedConfig.feedLine.take_position.compare(real_position):
-                # 打开吸嘴并返回 #TODO
+                # 打开吸嘴并返回
+                self.sendIOControl(self.robotClient.con_ios[0],1)
+                self.sendIOControl(self.robotClient.con_ios[1],1)
+                self.sendIOControl(self.robotClient.con_ios[2],1)
+                # TODO 检测是否通 不然报警
                 time.sleep(2)
                 self.feedStatus = FeedStatus.FSafeF
                 log.log_message(logging.INFO, Constant.str_feed_take_success)
@@ -178,7 +208,10 @@ class Feeding:
                 self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
                 pass
             if self.feedConfig.feedLine.drop_bag_position.compare(real_position):
-                # TODO 松开吸嘴
+                self.sendIOControl(self.robotClient.con_ios[0],0)
+                self.sendIOControl(self.robotClient.con_ios[1],0)
+                self.sendIOControl(self.robotClient.con_ios[2],0)
+                # TODO 检测是否断 不然报警
                 time.sleep(2)
                 self.feedConfig.num = self.feedConfig.num - 1
                 log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
@@ -192,6 +225,23 @@ class Feeding:
                     self.sendTargPosition(self.feedConfig.feedLine.safe_position)
 
 
+    def send_emergency_sound(self):
+       self.sendIOControl(Constant.IO_EmergencyPoint, 1)
+
+    def send_emergency_stop(self):
+        self.sendIOControl(Constant.IO_EmergencyPoint, 0)
+
+    def sendIOControl(self,IO_bit,IO_Status:int):
+        from Model.RobotModel import Instruction
+        IO_command = CMDInstructRequest()
+        io_instruction = Instruction()
+        io_instruction.IO = True
+        io_instruction.io_status = IO_Status
+        io_instruction.point = IO_bit  # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
+        IO_command.dsID = 'HCRemoteCommand'
+        IO_command.instructions.append(io_instruction)
+        self.robotClient.add_sendQuene(IO_command)
+        pass
 
     def sendTargPosition(self,real_position,move_type:MoveType=MoveType.WORLD ,speed = Constant.speed):
         from Model.RobotModel import Instruction

Constant.py

@@ -1,5 +1,7 @@
 import os
 
+IO_EmergencyPoint = 3
+bag_height = 10 # 一袋的高度
 position_accuracy = 0.05
 manual_adjust_accuracy = 1
 speed = 10
@@ -30,6 +32,11 @@ str_feed_broken_bag = '划袋'
 str_feed_drop = '移动扔空袋'
 str_feed_takePhoto_fail = '识别图像失败'
 str_feed_takePhoto_success = '识别图像成功'
+str_feed_takePhoto_new_line = '新的一排袋识别'
+str_feed_takePhoto_line = '一排袋最高的识别'
+str_feed_takePhoto_front_finish = '零星袋完成'
+str_feed_takePhoto_front = '零星袋识别'
+str_feed_takePhoto_move = '移动到抓料位置'
 str_feed_covert_success = '转换坐标成功'
 str_feed_covert_fail = '转换坐标失败'
 str_feed_error = '未知错误'
@@ -48,6 +55,7 @@ str_sys_setFeedNum = '设置投料次数'
 str_sys_feedNum_sub = '减少投料次数'
 str_sys_feedNum_add = '增加投料次数'
 str_sys_log_feedNum ='记录袋数失败'
+str_sys_emergencyStop = '按下急停'
 str_tcp_robot_connect_fail = '连接失败'
 str_tcp_robot_connect_success = '连接成功'
 str_tcp_robot_data_error = '数据解析错误'

MainWin.ui

@@ -4350,7 +4350,7 @@ background-color: #499c8a;
               </widget>
              </item>
              <item row="2" column="0">
-              <widget class="QPushButton" name="pushButton_17">
+              <widget class="QPushButton" name="pushButton_emergency">
                <property name="sizePolicy">
                 <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
                  <horstretch>0</horstretch>

Model/Position.py

@@ -46,3 +46,6 @@ class Real_Position(Position):
         self.V = V
         self.W = W
         return self
+
+    def to_string(self):
+        return "X:{:.3f},Y:{:.3f},Z:{:.3f},U:{:.3f},V:{:.3f},W:{:.3f}".format(self.X,self.Y,self.Z,self.U,self.V,self.W)

Model/RobotModel.py

@@ -162,7 +162,7 @@ class CMDInstructRequest:
     def __init__(self):
         self.dsID = 'www.hc-system.com.HCRemoteCommand'
         self.reqType = "AddRCC"
-        self.emptyList = '1'
+        self.emptyList = '1' #清空机械臂的远程命令数据
         self.instructions = []
 
     def toString(self):

README.md

@@ -0,0 +1,8 @@
+# 配置环境
+- 设置相机的IP地址  Seting.ini
+- 设置机械臂的IP地址 Seting.ini
+- 设置袋高  Constant.py bag_height
+- 设置五个拍照位置 界面
+- 设置N个固定位置 界面
+- 设置速度 界面
+- 转换矩阵的R矩阵的设置的常数需要确定

Seting.ini

@@ -46,8 +46,11 @@ photo_u5 = 0
 photo_v5 = 0
 photo_w5 = 0
 linecount = 2
-remain_linename = 2
+remain_linename = 0
-remain_count = 1
+remain_count = 0
+solenoid_valve1_addr=0
+solenoid_valve2_addr=0
+solenoid_valve3_addr=0
 
 [Camera_Feed]
 ipaddress = 127.0.0.1

main.py

@@ -155,6 +155,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
         self.pushButton_set_p5.clicked.connect(self.set_p5_button_click)
 
         self.pushButton_clearAlarm.clicked.connect(self.send_clear_alarm_command)
+        self.pushButton_emergency.clicked.connect(self.send_emergency_alarm_command)
         self.pushButton_reset.clicked.connect(self.send_reset_button_click)
         self.pushButton_speed.setText(str(Constant.speed))
 
@@ -221,9 +222,12 @@ class MainWindow(QMainWindow, Ui_MainWindow):
                        int(self.configReader.get('Robot_Feed', 'photo_u5')),int(self.configReader.get('Robot_Feed', 'photo_v5')),int(self.configReader.get('Robot_Feed', 'photo_w5'))
                        )
                       ]
+        solenoid_valve1_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve1_addr'))
+        solenoid_valve2_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve2_addr'))
+        solenoid_valve3_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve3_addr'))
         #TODO
         #dropDelay_time = int(self.configReader.get('Robot_Feed', 'dropDelay_time'))
-        self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address)
+        self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address,[solenoid_valve1_addr, solenoid_valve2_addr, solenoid_valve3_addr])
         self.feeding = Feeding(self.robotClient, self.detection)  # 临时
         self.last_time = time.time()
         self.remain_lineName = self.configReader.get('Robot_Feed', 'remain_lineName')
@@ -473,6 +477,9 @@ class MainWindow(QMainWindow, Ui_MainWindow):
             return
 
     def send_startFeed_button_click(self):
+        if self.feeding.feedStatus != FeedStatus.FNone:
+            self.show_infomessage_box("正在执行")
+            return
         # 触发自动运行
         if self.robotClient.status_model.curMode != 7:
             self.send_switch_tool_command()
@@ -1013,7 +1020,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
 
     def set_run_status_button(self, isRuning: bool):
         self.pushButton_pauseFeed.setText("暂停")
-
+        ##self.pushButton_startFeed.setEnabled(False)
 
         # if isRuning:
         #     self.pushButton_pauseFeed.show()
@@ -1026,12 +1033,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
 
     def send_clear_auto_command(self):
         clear_command = CMDInstructRequest()
-        io_instruction= Instruction()
-        io_instruction.IO = True
-        io_instruction.point=0 #{"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
-        io_instruction.delay=1
-        clear_command.dsID = 'HCRemoteCommand'
-        clear_command.instructions.append(io_instruction)
         request_command = clear_command.toString()
         log.log_message(logging.INFO, Constant.str_sys_clearAlarm)
         self.command_quene.put(request_command)
@@ -1071,15 +1072,19 @@ class MainWindow(QMainWindow, Ui_MainWindow):
         print(request_command)
         self.robotClient.add_sendQuene(request_command)
 
-    def send_clear_alarm_command(self, pause: bool):
+    def send_emergency_alarm_command(self):
-        switch_command = CMDRequest()
+        stop_command = CMDRequest()
-        switch_command.cmdData.append("modifyGSPD")
+        stop_command.cmdData.append("actionStop")
-        switch_command.cmdData.append("2")
+        stop_command.cmdData.append("1")
-        request_command = switch_command.toString()
+        request_command = stop_command.toString()
-        print(request_command)
-
         self.robotClient.add_sendQuene(request_command)
+        self.feeding.send_emergency_sound()
+        log.log_message(logging.INFO, Constant.str_sys_emergencyStop)
+        #TODO
 
+
+    def send_clear_alarm_command(self, pause: bool):
+        self.send_start_tool_command()
         pause_command = CMDRequest()
         pause_command.cmdData.append("clearAlarmContinue")
         pause_command.cmdData.append("1")

test7.py

@@ -0,0 +1,24 @@
+class Position:
+    def __init__(self):
+        self.X = 0.0
+        self.Y = 0.0
+        self.Z = 0.0
+        self.U = 0.0
+        self.V = 0.0
+        self.W = 0.0
+
+    def init_data(self, X, Y, Z, U, V, W):
+        self.X = X
+        self.Y = Y
+        self.Z = Z
+        return self
+
+detect_pos_list = []
+detect_pos_list.append(Position().init_data(0.0, 0.0, 3, 0.0, 0.0, 0.0))
+detect_pos_list.append(Position().init_data(0.0, 0.0, 1, 0.0, 0.0, 0.0))
+detect_pos_list.append(Position().init_data(0.0, 0.0, 8, 0.0, 0.0, 0.0))
+z_diff, max_z_index = (lambda pts: (
+    max(pts, key=lambda p: p.Z).Z - min(pts, key=lambda p: p.Z).Z,
+    pts.index(max(pts, key=lambda p: p.Z))
+))(detect_pos_list)
+print(z_diff, max_z_index)

ui_MainWin.py

@@ -2740,11 +2740,11 @@ class Ui_MainWindow(object):
 
         self.gridLayout_2.addWidget(self.pushButton_pauseFeed, 0, 1, 1, 2)
 
-        self.pushButton_17 = QPushButton(self.frame_21)
+        self.pushButton_emergency = QPushButton(self.frame_21)
-        self.pushButton_17.setObjectName(u"pushButton_17")
+        self.pushButton_emergency.setObjectName(u"pushButton_emergency")
-        sizePolicy5.setHeightForWidth(self.pushButton_17.sizePolicy().hasHeightForWidth())
+        sizePolicy5.setHeightForWidth(self.pushButton_emergency.sizePolicy().hasHeightForWidth())
-        self.pushButton_17.setSizePolicy(sizePolicy5)
+        self.pushButton_emergency.setSizePolicy(sizePolicy5)
-        self.pushButton_17.setStyleSheet(u"*{\n"
+        self.pushButton_emergency.setStyleSheet(u"*{\n"
 "background-color: #FFF000;\n"
 "font: 9pt \"\u6977\u4f53\";\n"
 "border-radius: 10px;\n"
@@ -2756,9 +2756,9 @@ class Ui_MainWindow(object):
 "\n"
 "")
         icon10 = QIcon(QIcon.fromTheme(QIcon.ThemeIcon.WindowClose))
-        self.pushButton_17.setIcon(icon10)
+        self.pushButton_emergency.setIcon(icon10)
 
-        self.gridLayout_2.addWidget(self.pushButton_17, 2, 0, 1, 1)
+        self.gridLayout_2.addWidget(self.pushButton_emergency, 2, 0, 1, 1)
 
         self.pushButton_clearAlarm = QPushButton(self.frame_21)
         self.pushButton_clearAlarm.setObjectName(u"pushButton_clearAlarm")
@@ -3136,7 +3136,7 @@ class Ui_MainWindow(object):
         self.pushButton_SubNum.setText(QCoreApplication.translate("MainWindow", u"\u6263\u4e00\u888b", None))
         self.pushButton_startFeed.setText(QCoreApplication.translate("MainWindow", u"\u542f\u52a8", None))
         self.pushButton_pauseFeed.setText(QCoreApplication.translate("MainWindow", u"\u6682\u505c", None))
-        self.pushButton_17.setText(QCoreApplication.translate("MainWindow", u"\u6025\u505c", None))
+        self.pushButton_emergency.setText(QCoreApplication.translate("MainWindow", u"\u6025\u505c", None))
         self.pushButton_clearAlarm.setText(QCoreApplication.translate("MainWindow", u"\u6e05\u9664\u62a5\u8b66", None))
         self.pushButton_stopFeed.setText(QCoreApplication.translate("MainWindow", u"\u505c\u6b62", None))
         self.pushButton_reset.setText(QCoreApplication.translate("MainWindow", u"\u590d\u4f4d", None))