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

CU/Catch.py

@@ -29,10 +29,6 @@ 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:

CU/Detect.py

@@ -37,8 +37,9 @@ class Detect:
                 return
             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

@@ -19,7 +19,7 @@ from Model.Position import Real_Position, Detection_Position
 from enum import Enum, IntEnum
 from COM.COM_Robot import RobotClient, DetectType
 from Model.RobotModel import CMDInstructRequest, MoveType
-from Trace.handeye_calibration import getxyz
+from Trace.handeye_calibration import getPosition
 from Util.util_math import get_distance
 from Util.util_time import CRisOrFall
 from Vision.camera_coordinate_dete import Detection
@@ -133,16 +133,15 @@ class FeedLine:
         for i in range(len(self.feeding_to_end)):
             if self.feeding_to_end[i].status == FeedStatus.FTake.value:
                 if position != None:
-                    xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c)
-                    befor_take_position = Real_Position().init_position(xyz[0],
-                                                                        xyz[1],
-                                                                        xyz[2],
+                    befor_take_position = Real_Position().init_position(position.X,
+                                                                        position.Y,
+                                                                        position.Z+dynamic_height,
                                                                         position.U,
                                                                         position.V,
                                                                         position.W)
-                    after_take_position = Real_Position().init_position(xyz[0],
-                                                                        xyz[1],
-                                                                        xyz[2],
+                    after_take_position = Real_Position().init_position(position.X,
+                                                                        position.Y,
+                                                                        position.Z+dynamic_height,
                                                                         position.U,
                                                                         position.V,
                                                                         position.W)

Config/FeedLine.ini

@@ -59,12 +59,12 @@ status = 6
 linetype = 0
 
 [Position9]
-x = 1430.492554
-y = 1765.717407
-z = 1832.536255
-u = 1.57702
-v = 4.174215
-w = -87.506783
+x = 1427.58313
+y = 1724.46875
+z = 1831.32312
+u = 1.923308
+v = 3.454441
+w = -87.299934
 id = 9
 order = 9
 lineid = 1
@@ -72,12 +72,12 @@ status = 7
 linetype = 0
 
 [Position10]
-x = 1375.01416
-y = 1702.021973
-z = 2117.369385
-u = 8.211453
-v = 4.232689
-w = -100.153625
+x = 1339.699585
+y = 1702.385742
+z = 2197.976318
+u = 9.554496
+v = 7.15853
+w = -99.243294
 id = 10
 order = 10
 lineid = 1
@@ -202,12 +202,12 @@ status = 8
 linetype = 0
 
 [Position11]
-x = 1244.785156
-y = 1848.819702
-z = 2307.201416
-u = -34.622036
-v = 19.413801
-w = -129.918442
+x = 1331.740112
+y = 1787.211548
+z = 2307.181641
+u = -34.621368
+v = 19.412609
+w = -132.657135
 id = 11
 order = 12
 lineid = 1

Model/Position.py

@@ -10,6 +10,10 @@ class Position:
         self.U = 0.0
         self.V = 0.0
         self.W = 0.0
+        self.a = 0.0
+        self.b = 0.0
+        self.c = 0.0
+
 
     def compare(self,position,is_action=False):
         distance = math.sqrt((self.X-position.X)**2+

Seting.ini

@@ -59,9 +59,9 @@ smooth = 9
 dynamic_height = 350.0
 
 [Speed]
-debug_speed = 50
-feed_speed = 550
-reset_speed = 35
+debug_speed = 80
+feed_speed = 100
+reset_speed = 80
 
 [Origin]
 x = 204.996765

Trace/handeye_calibration.py

@@ -76,7 +76,7 @@ def getPosition(x,y,z,a,b,c,rotation,points):
     # 单位化方向向量
     short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
 
-    delta = 0#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
+    delta = -200#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
     angle = np.asarray([a,b,c])
     noraml = camera2robot[:3, :3]@angle
     normal_vector = noraml / np.linalg.norm(noraml)
@@ -90,7 +90,7 @@ def getxyz(x,y,z,a,b,c):
     target = np.asarray([x, y, z, 1])
     camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ')
     target_position_raw = np.dot(camera2robot, target)
-    delta = -200  # 沿法向量方向抬高和压低，-指表示抬高，+值表示压低
+    delta = -500  # 沿法向量方向抬高和压低，-指表示抬高，+值表示压低
     angle = np.asarray([a, b, c])
     noraml = camera2robot[:3, :3] @ angle
     normal_vector = noraml / np.linalg.norm(noraml)