修改钢针到达抓料点位后再收缩和在震动点不走的问题

CU/Catch.py

@@ -29,6 +29,10 @@ class Catch:
         self.shake_Q = False
     def run(self):
         if self.catch_status == CatchStatus.CNone:
+            self.shake_continue.SetReset()
+            # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
+            close(0, 1, 0)
+            self.is_send_take_command = False
             return
 
         if self.catch_status == CatchStatus.CTake:
@@ -64,20 +68,21 @@ class Catch:
 
         if self.catch_status == CatchStatus.CShake:     # 1500
             self.shake_Q = not self.shake_Q   #   10
-            if not self.shake_continue.Q(True, 4000):
+            if not self.shake_continue.Q(True, 6000):
                 if self.shake_Q:
                     open(0, 1, 0)
                 else:
                     close(0, 1, 0)
             else:
                 self.shake_continue.SetReset()
-                self.catch_status = CatchStatus.COk
+                #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)
                 close(0, 1, 0)
                 print("震动结束")
 
         if self.catch_status == CatchStatus.COk :
+            self.shake_continue.SetReset()
             # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
             open(1,0,0)
             close(0, 1, 0)

CU/Detect.py

@@ -38,7 +38,7 @@ class Detect:
             target_position, noraml_base = getPosition(*xyz, *uvw, None, points)
 
             position = Real_Position().init_position(*target_position[:3], *noraml_base[:3])
-            position.Z = position.Z + 200
+            position.Z = position.Z
             self.detect_position = position
             self.detect_status = DetectStatus.DOk
 

CU/Feeding.py

@@ -485,18 +485,23 @@ class Feeding(QObject):
             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.COk:
-                    self.catch.catch_status = CatchStatus.CNone
-                    self.next_position()
-                    return
                 if self.catch.catch_status == CatchStatus.CNone:
                     self.catch.catch_status = CatchStatus.CShake
                     return
                 if self.catch.catch_status == CatchStatus.CShake:
+                    # if self.feedConfig.feedLine.feeding_to_end[
+                    #     self.feedConfig.feedLine.feeding2end_pos_index + 1].status != FeedStatus.FShake:
+                    #     self.catch.catch_status = CatchStatus.COk
+                    # else:
+                    self.catch.shake_continue.SetReset()
+                    self.next_position()
+                    if self.feedStatus!=FeedStatus.FShake:
+                        self.catch.catch_status = CatchStatus.CNone
                     return
 
 
 
+
         elif self.feedStatus == FeedStatus.FDropBag: