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

CU/Catch.py

@@ -36,7 +36,7 @@ class Catch:
                 # 本身IO
                 ##self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
                 # 网络继电器
-                open(1, 0, 0)
+                close(1, 0, 0)
                 self.is_send_take_command = True
 
         if self.catch_status == CatchStatus.CDrop:
@@ -45,15 +45,16 @@ class Catch:
                 # self.robotClient.sendIOControl(self.robotClient.con_ios[0], 0)
                 # 网络继电器
                 close(1, 0, 0)
+                time.sleep(1)
                 for _ in range(self.drop_count):
                     # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1, delay=self.robotClient.time_delay_put)
-                    open(0, 1, 0)
+                    open(0, 0, 1)
                     time.sleep(self.robotClient.time_delay_put) # 会造成这个时间点 其他命令插入不进去 需要另开线程
-                    close(0, 1, 0)
+                    close(0, 0, 1)
                     # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
 
                 # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1)
-                open(0, 1, 0)
+                close(0, 0, 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]):
@@ -63,23 +64,25 @@ class Catch:
 
         if self.catch_status == CatchStatus.CShake:     # 1500
             self.shake_Q = not self.shake_Q   #   10
-            if not self.shake_continue.Q(True, 600):
+            if not self.shake_continue.Q(True, 4000):
                 if self.shake_Q:
-                    open(0, 0, 1)
+                    open(0, 1, 0)
                 else:
-                    close(0, 0, 1)
+                    close(0, 1, 0)
             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)
-                close(0, 0, 1)
+                close(0, 1, 0)
                 print("震动结束")
 
         if self.catch_status == CatchStatus.COk :
             # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
+            open(1,0,0)
             close(0, 1, 0)
             close(0, 0, 1)
+            self.is_send_take_command = False
             pass
 
 

CU/EMV.py

@@ -76,8 +76,8 @@ def close(grasp, shake, throw):
             time.sleep(0.5)
 
 # 关闭电磁阀
-# open(True, False, False)  # 参数传True和False
+# open(False, False, True)  # 参数传True和False
-# close(False,False,True)
+# close(True,False,True)
 # for i in range(10):
 #     open(False,True,True)
 #     close(True,True,True)

CU/Feeding.py

@@ -448,8 +448,13 @@ class Feeding(QObject):
                     #self.feedConfig.feedLine.set_take_position(None)
                     # time.sleep(self.robotClient.time_delay_take)
                     self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
-                    self.next_position()
+                    if self.catch.catch_status == CatchStatus.COk:
-                    self.catch.catch_status = CatchStatus.COk
+                        self.next_position()
+                        self.catch.catch_status = CatchStatus.CNone
+                        return
+                    if self.catch.catch_status == CatchStatus.CTake:
+                        self.catch.catch_status = CatchStatus.COk
+
 
             else:
                 self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
@@ -483,6 +488,7 @@ class Feeding(QObject):
                 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

Config/FeedLine.ini

@@ -92,7 +92,7 @@ u = 5.812903
 v = 5.431066
 w = -168.01712
 id = 12
-order = 11
+order = 14
 lineid = 1
 status = 9
 linetype = 0
@@ -188,3 +188,42 @@ lineid = 1
 status = 3
 linetype = 0
 
+[Position4]
+x = 1556.585449
+y = 1663.580322
+z = 2331.091553
+u = 48.062508
+v = -0.372809
+w = -96.427994
+id = 4
+order = 11
+lineid = 1
+status = 8
+linetype = 0
+
+[Position11]
+x = 1244.785156
+y = 1848.819702
+z = 2307.201416
+u = -34.622036
+v = 19.413801
+w = -129.918442
+id = 11
+order = 12
+lineid = 1
+status = 8
+linetype = 0
+
+[Position17]
+x = 1364.639404
+y = 1892.286743
+z = 2276.495361
+u = 11.343127
+v = 48.749588
+w = -110.22995
+id = 17
+order = 13
+lineid = 1
+status = 8
+linetype = 0
+

Trace/com_pose.txt

@@ -1,4 +1,4 @@
-9.4566884811714796e-02 -9.9470945966114444e-01 4.0127725032944608e-02 4.1471010091895931e+02
+1.1224831479369565e-01 -9.9366361198855646e-01 5.7395152958431492e-03 4.1812293824507674e+02
--9.9551731828304890e-01 -9.4428128820258375e-02 5.3435988155243787e-03 1.9335993881936060e+03
+-9.9269850944334537e-01 -1.1239223491705273e-01 -4.3791036517882603e-02 1.9260165135520242e+03
--1.5261424109928572e-03 -4.0453173929085366e-02 -9.9918028231904499e-01 2.7052051690106582e+03
+4.4158636470522372e-02 -7.8213822690929326e-04 -9.9902422547446690e-01 2.7083490666098191e+03
-0 0 0 1
+0. 0. 0. 1.

Trace/com_pose2.txt

@@ -1,4 +1,4 @@
-9.6751729364544639e-02 -9.9459449602428807e-01 3.7694712403242861e-02 4.1708226127179734e+02
+1.1224831479369565e-01 -9.9366361198855646e-01 5.7395152958431492e-03 4.1812293824507674e+02
--9.9527317207468335e-01 -9.6998182736478769e-02 -4.7608291523444628e-03 1.9086796578832980e+03
+-9.9269850944334537e-01 -1.1239223491705273e-01 -4.3791036517882603e-02 1.9260165135520242e+03
-8.3914130733227475e-03 -3.7055917530319504e-02 -9.9927796091108601e-01 2.7052748714031904e+03
+4.4158636470522372e-02 -7.8213822690929326e-04 -9.9902422547446690e-01 2.7083490666098191e+03
-0. 0. 0. 1
+0. 0. 0. 1.