🚑 update 更新抓取和扔料

COM/COM_Robot.py

@@ -107,12 +107,14 @@ class RobotClient(TCPClient):
     def send_emergency_stop(self):
         self.sendIOControl(Constant.IO_EmergencyPoint, 0)
 
-    def sendIOControl(self, IO_bit, IO_Status: int):#
+    def sendIOControl(self, IO_bit, IO_Status: int, delay=0,emptyList='0'):#
 
         IO_command = CMDInstructRequest()
+        IO_command.emptyList = emptyList
         io_instruction = Instruction()
         io_instruction.IO = True
         io_instruction.io_status = IO_Status
+        io_instruction.delay = delay
         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)

CU/Catch.py

@@ -1,3 +1,4 @@
+import time
 from enum import Enum
 
 import Constant
@@ -17,27 +18,46 @@ class Catch:
         self.robotClient = robotClient
         self.catch_status = CatchStatus.CNone
         self.shake_continue = CTon()
+        self.drop_continue = CTon()
+        self.take_continue = CTon()
         self.shakePulse = CClockPulse()
         self.shake_count = 5
+        self.drop_count = 2
+        self.is_send_command = False
+        self.is_send_take_command = False
+        self.shake_Q = False
     def run(self):
         if self.catch_status == CatchStatus.CNone:
             return
 
         if self.catch_status == CatchStatus.CTake:
-            self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
+            if not self.is_send_take_command:
-            if self.robotClient.check_outputQ(self.robotClient.con_ios[0]) or Constant.Debug:
+                self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
-                self.catch_status = CatchStatus.COk
+                self.is_send_take_command = True
+            else:
+                if self.take_continue.Q(True,self.robotClient.time_delay_take*1000):
+                    self.is_send_take_command = False
+                    self.catch_status = CatchStatus.COk
 
         if self.catch_status == CatchStatus.CDrop:
-            self.robotClient.sendIOControl(self.robotClient.con_ios[0],0)
+            if not self.is_send_command:
-            self.robotClient.sendIOControl(self.robotClient.con_ios[1],1)
+                self.robotClient.sendIOControl(self.robotClient.con_ios[0], 0)
-            # if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
+                for _ in range(self.drop_count):
-            self.catch_status = CatchStatus.COk
+                    self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1, delay=self.robotClient.time_delay_put)
+                    self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
+
+                self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1)
+                self.is_send_command = True
+            if self.drop_continue.Q(True,self.robotClient.time_delay_put*1000*self.drop_count):
+                # if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
+                self.is_send_command = False
+                self.catch_status = CatchStatus.COk
+                self.drop_continue.SetReset()
+
         if self.catch_status == CatchStatus.CShake:
-            if not self.shake_continue.Q(True, 1500):
+            if not self.shake_continue.Q(True, 600):       # 1500
-                shakeQ = self.shakePulse.Q(True, 50, 50)
+                self.shake_Q = not self.shake_Q   #   10
-                self.robotClient.sendIOControl(self.robotClient.con_ios[2], 1 if shakeQ else 0)
+                self.robotClient.sendIOControl(self.robotClient.con_ios[2], 1 if self.shake_Q else 0)
-                print("正在震动" if shakeQ else "震动结束")
             else:
                 self.shake_continue.SetReset()
                 self.catch_status = CatchStatus.COk
@@ -46,7 +66,7 @@ class Catch:
                 print("震动结束")
 
         if self.catch_status == CatchStatus.COk:
-            self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
+            self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
             pass
 
 

CU/Feeding.py

@@ -208,6 +208,7 @@ class Feeding(QObject):
         self.init_detection_image()
         self.pause = False
         self.cRis_photo = CRisOrFall()
+        self.cRis_shake = CRisOrFall()
         self.feed_Mid_Status = FeedMidStatus.FMid_Start
         self.is_reverse = False
         # 复位集合
@@ -316,9 +317,10 @@ class Feeding(QObject):
 
 
         elif self.feedStatus == FeedStatus.FMid:
-            self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
+
             feed_pos = self.get_current_position(self.is_reverse)
             if feed_pos.get_position().compare(real_position):
+                self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
                 self.next_position(self.is_reverse)
 
 
@@ -450,7 +452,7 @@ class Feeding(QObject):
                     if self.catch.catch_status == CatchStatus.COk :
                         self.catch.catch_status = CatchStatus.CNone
                         #self.feedConfig.feedLine.set_take_position(None)
-                        time.sleep(self.robotClient.time_delay_take)
+                        # time.sleep(self.robotClient.time_delay_take)
                         self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
                         self.next_position()
 
@@ -466,21 +468,23 @@ class Feeding(QObject):
 
 
         elif self.feedStatus == FeedStatus.FBroken1:
-            self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
+
             if self.get_current_position().get_position().compare(real_position):
+                self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
                 self.next_position()
 
 
         elif self.feedStatus == FeedStatus.FBroken2:
-            self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
+
             if self.get_current_position().get_position().compare(real_position):
+                self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
                 self.next_position()
 
         elif self.feedStatus == FeedStatus.FShake:
-            self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
+
             if self.get_current_position().get_position().compare(real_position,is_action=True):
                 # TODO 震动方案
-
+                self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
 
                 if self.catch.catch_status == CatchStatus.CNone:
                     self.catch.catch_status = CatchStatus.CShake
@@ -489,7 +493,6 @@ class Feeding(QObject):
                     return
                 if self.catch.catch_status == CatchStatus.COk:
                     self.catch.catch_status = CatchStatus.CNone
-                    time.sleep(self.robotClient.time_delay_shake)
                     self.next_position()
 
         elif self.feedStatus == FeedStatus.FDropBag:
@@ -507,7 +510,7 @@ class Feeding(QObject):
                     return
                 if self.catch.catch_status == CatchStatus.COk:
                     self.catch.catch_status = CatchStatus.CNone
-                    time.sleep(self.robotClient.time_delay_put)
+                    # time.sleep(self.robotClient.time_delay_put)
                     # 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.detect.detect_status == DetectStatus.DNone:
@@ -587,9 +590,6 @@ class Feeding(QObject):
             self.reset_status = ResetStatus.RNone
             self.send_emergency_stop()
 
-
-
-
     def return_original_position(self):
 
         self.run_reverse = True
@@ -672,6 +672,10 @@ class Feeding(QObject):
         self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
         pass
 
+    def sendTargPosition1(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
+        pass
+
+
     def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
         position_instruction = Instruction()
         position_instruction.speed = speed
@@ -705,10 +709,12 @@ class Feeding(QObject):
 
 
         try:
+            print('fuck1',log_str)
             self.log_signal.emit(logging.INFO, log_str)
+            print('fuck2',log_str)
         except:
             return
-
+        print(request_command)
         self.robotClient.add_sendQuene(request_command)
         pass
     # def get_take_position(self):
@@ -778,6 +784,16 @@ class Feeding(QObject):
                                       move_type=MoveType.AXIS)
 
         else:
+            # if not reverse and self.feedStatus == FeedStatus.FShake:
+            # if not reverse:
+            #     if self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, True):
+            #         pass
+            #     elif self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, False):
+            #         self.feedStatus = FeedStatus.FShake
+            #         self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
+            #         return
+
+
             self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
 
     def get_current_position(self,is_reverse=False):

Config/FeedLine.ini

@@ -19,39 +19,26 @@ lineid = 2
 status = 3
 linetype = 0
 
-[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
-
 [Position1]
-x = 348.84654634488436
+x = 7.0
-y = 1836.958560527403
+y = 50.0
-z = 223.84778176988266
+z = 1.0
-u = 2.3775411141593388
+u = 12.0
-v = -0.7172354339862681
+v = 0.0
-w = 95.0492701673298
+w = 1.0
 id = 1
 order = 4
 lineid = 1
-status = 5
+status = 3
 linetype = 0
 
 [Position7]
-x = 2218.886475
+x = 7.0
-y = 483.322144
+y = 50.0
-z = 1371.784302
+z = 1.0
-u = 8.190439
+u = 12.0
-v = 7.692511
+v = 0.0
-w = -143.628922
+w = 1.0
 id = 7
 order = 0
 lineid = 1
@@ -59,94 +46,68 @@ status = 3
 linetype = 0
 
 [Position8]
-x = 807.391785
+x = 7.0
-y = 2337.584473
+y = 50.0
-z = 1852.736816
+z = 1.0
-u = -0.412497
+u = 12.0
-v = 9.080836
+v = 0.0
-w = -110.687553
+w = 1.0
 id = 8
-order = 5
+order = 9
 lineid = 1
 status = 6
 linetype = 0
 
 [Position9]
-x = 1759.061523
+x = 7.0
-y = 1738.14978
+y = 50.0
-z = 1852.472412
+z = 1.0
-u = 0.723375
+u = 12.0
-v = 9.708878
+v = 0.0
-w = -126.572548
+w = 1.0
 id = 9
-order = 6
+order = 10
 lineid = 1
 status = 7
 linetype = 0
 
 [Position10]
-x = 1759.061523
+x = 7.0
-y = 1738.14978
+y = 50.0
-z = 1852.472412
+z = 1.0
-u = 0.723375
+u = 12.0
-v = 9.708878
+v = 0.0
-w = -126.572548
+w = 1.0
 id = 10
-order = 7
+order = 11
 lineid = 1
 status = 8
 linetype = 0
 
-[Position11]
-x = 1960.667847
-y = 837.290405
-z = 1688.202637
-u = -0.347183
-v = 0.356827
-w = -147.846161
-id = 11
-order = 8
-lineid = 1
-status = 3
-linetype = 0
-
 [Position12]
-x = 1643.540039
+x = 7.0
-y = -24.557989
+y = 50.0
-z = 995.504272
+z = 1.0
-u = 0.167181
+u = 12.0
-v = 2.42627
+v = 0.0
-w = 177.088989
+w = 1.0
 id = 12
-order = 9
+order = 12
 lineid = 1
 status = 9
 linetype = 0
 
 [Position2]
-x = 762.708984
+x = 7.0
-y = 1915.674561
+y = 50.0
-z = 1265.639648
+z = 1.0
-u = 12.478849
+u = 12.0
-v = 1.611004
+v = 0.0
-w = -84.483177
+w = 1.0
 id = 2
 order = 3
 lineid = 1
-status = 3
+status = 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]
@@ -162,3 +123,81 @@ lineid = 2
 status = 2
 linetype = 0
 
+[Position13]
+x = 7.0
+y = 50.0
+z = 1.0
+u = 12.0
+v = 0.0
+w = 1.0
+id = 13
+order = 1
+lineid = 1
+status = 3
+linetype = 0
+
+[Position14]
+x = 7.0
+y = 50.0
+z = 1.0
+u = 12.0
+v = 0.0
+w = 1.0
+id = 14
+order = 5
+lineid = 1
+status = 3
+linetype = 0
+
+[Position5]
+x = 7.0
+y = 50.0
+z = 1.0
+u = 12.0
+v = 0.0
+w = 1.0
+id = 5
+order = 6
+lineid = 1
+status = 5
+linetype = 0
+
+[Position15]
+x = 7.0
+y = 50.0
+z = 1.0
+u = 12.0
+v = 0.0
+w = 1.0
+id = 15
+order = 2
+lineid = 1
+status = 2
+linetype = 0
+
+[Position4]
+x = 7.0
+y = 50.0
+z = 1.0
+u = 12.0
+v = 0.0
+w = 1.0
+id = 4
+order = 7
+lineid = 1
+status = 3
+linetype = 0
+
+[Position16]
+x = 7.0
+y = 50.0
+z = 1.0
+u = 12.0
+v = 0.0
+w = 1.0
+id = 16
+order = 8
+lineid = 1
+status = 3
+linetype = 0
+

Model/RobotModel.py

@@ -149,7 +149,6 @@ class Instruction:
         self.m5_p = 0.0
         self.ckStatus = '0x3F'
         self.speed = 10
-        self.delay = 1.0
         self.smooth = 0
         self.tool=2
         self.IO = False
@@ -181,7 +180,7 @@ class CMDInstructRequest:
     def __init__(self):
         self.dsID = 'www.hc-system.com.HCRemoteCommand'
         self.reqType = "AddRCC"
-        self.emptyList = '1' #清空机械臂的远程命令数据
+        self.emptyList = '0' #清空机械臂的远程命令数据
         self.instructions = []
 
     def toString(self):

Seting.ini

@@ -47,7 +47,7 @@ photo_v5 = 0.0
 photo_w5 = 1.0
 linecount = 2
 remain_linename = 1
-remain_count = 998
+remain_count = 999
 io_take_addr = 8
 io_zip_addr = 11
 io_shake_addr = 12
@@ -55,10 +55,12 @@ takedelay = 0.2
 putdelay = 0.1
 shakedelay = 1.0
 max_angle_interval = 20
+smooth = 9
+dynamic_height = 520.0
 
 [Speed]
 debug_speed = 50
-feed_speed = 10
+feed_speed = 550
 reset_speed = 35
 
 [Origin]

Util/util_language.py

@@ -0,0 +1,10 @@
+from PyQt5.QtCore import QTranslator, QLocale, QLibraryInfo
+
+
+def show_translated_message_box(app):
+     # 加载翻译文件
+    translator = QTranslator()
+    locale = QLocale.system().name()  # 获取系统语言
+    qt_translations_path = QLibraryInfo.path(QLibraryInfo.TranslationsPath)
+    translator.load(f"qtbase_{locale}", qt_translations_path)
+    app.installTranslator(translator)

main.py

@@ -449,12 +449,12 @@ class MainWindow(QMainWindow, Ui_MainWindow):
                         float(self.tableWidget_line_positions.item(row_i-1, 5).text()),
                         float(self.tableWidget_line_positions.item(row_i-1, 6).text()),
                     )
-                    self.send_position_command(x1=p1.X, x2=p1.Y, x3=p1.Z, x4=p1.X, x5=p1.Y, x6=p1.Z, move_type=MoveType.Cure, speed=self.robotClient.debug_speed, p2=p2)
+                    self.send_position_command(x1=p1.X, x2=p1.Y, x3=p1.Z, x4=p1.U, x5=p1.V, x6=p1.W, move_type=MoveType.Cure, speed=self.robotClient.debug_speed, p2=p2)
                     return
-            self.send_position_command(x1=p1.X, x2=p1.Y, x3=p1.Z, x4=p1.X, x5=p1.Y, x6=p1.Z, move_type=MoveType.WORLD,speed=self.robotClient.debug_speed)
+            self.send_position_command(x1=p1.X, x2=p1.Y, x3=p1.Z, x4=p1.U, x5=p1.V, x6=p1.W, move_type=MoveType.WORLD,speed=self.robotClient.debug_speed)
             return
         if lineType == 4: # 自由路径
-            self.send_position_command(x1=p1.X, x2=p1.Y, x3=p1.Z, x4=p1.X, x5=p1.Y, x6=p1.Z,move_type=MoveType.AXIS,speed=self.robotClient.debug_speed)
+            self.send_position_command(x1=p1.X, x2=p1.Y, x3=p1.Z, x4=p1.U, x5=p1.V, x6=p1.W, move_type=MoveType.AXIS,speed=self.robotClient.debug_speed)
             return
 
     def tabel_get_position(self,row_i):
@@ -1653,6 +1653,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
 
     def send_clear_auto_command(self):
         clear_command = CMDInstructRequest()
+        clear_command.emptyList = '1'
         request_command = clear_command.toString()
         log.log_message(logging.INFO, Constant.str_sys_clearAlarm)
         self.robotClient.add_sendQuene(request_command)