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

.idea/AutoControlSystem.iml

@@ -2,7 +2,7 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="rob" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.9" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
   <component name="PyDocumentationSettings">

.idea/misc.xml

@@ -3,5 +3,5 @@
   <component name="Black">
     <option name="sdkName" value="rob" />
   </component>
-  <component name="ProjectRootManager" version="2" project-jdk-name="rob" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9" project-jdk-type="Python SDK" />
 </project>

CU/Catch.py

@@ -1,10 +1,11 @@
+#!/usr/bin/python3
 import time
 from enum import Enum
 
 import Constant
 from COM.COM_Robot import RobotClient
 from Util.util_time import CClockPulse, CTon
-from EMV import *
+from .EMV import *
 
 class CatchStatus(Enum):
     CNone = 0
@@ -37,10 +38,6 @@ class Catch:
                 # 网络继电器
                 open(1, 0, 0)
                 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:
             if not self.is_send_command:

CU/Detect.py

@@ -28,7 +28,7 @@ class Detect:
                 return
             _, img, xyz, uvw, points = 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)
+                                                                   save_img_point=1, Height_reduce=30, width_reduce=30)
 
             self.detection_image = img.copy()
             if xyz == None or uvw == None or points == None:

CU/EMV.py

@@ -9,7 +9,7 @@ PORT = 50000
 # 电磁阀控制报文
 valve_commands = {
     1: {
-        'open': '00000000000601050000FF00',
+        'open': '00000000000601050000FF00',#钢针
         'close': '000000000006010500000000',
     },
     2: {
@@ -46,34 +46,38 @@ def send_command(command):
             return False
 
 # 控制电磁阀打开
-def open(grasp, throw, shake):
+def open(grasp, shake, throw):
     if grasp:
-        print("打开电磁阀 1")  # 抓取
+        print("打开电磁阀 1")  # 钢针关闭
         if send_command(valve_commands[1]['open']):
-            time.sleep(1)  # 等待 1 秒
+            time.sleep(1)
-    if throw:
-        print("打开电磁阀 2")  # 扔袋
-        if send_command(valve_commands[2]['open']):
-            time.sleep(1)  # 等待 1 秒
     if shake:
-        print("打开电磁阀 3")  # 震动
+        print("打开电磁阀 2")  # 震动
+        if send_command(valve_commands[2]['open']):
+            time.sleep(0.05)
+    if throw:
+        print("打开电磁阀 3")  # 扔袋
         if send_command(valve_commands[3]['open']):
-            time.sleep(1)  # 等待 1 秒
+            time.sleep(0.5)
 
 # 控制电磁阀关闭
-def close(grasp, throw, shake):
+def close(grasp, shake, throw):
     if grasp:
-        print("关闭电磁阀 1")
+        print("关闭电磁阀 1")#钢针打开
         if send_command(valve_commands[1]['close']):
-            time.sleep(1)  # 等待 1 秒
+            time.sleep(1)
-    if throw:
+    if shake:
         print("关闭电磁阀 2")
         if send_command(valve_commands[2]['close']):
-            time.sleep(1)  # 等待 1 秒
+            time.sleep(0.05)
-    if shake:
+    if throw:
         print("关闭电磁阀 3")
         if send_command(valve_commands[3]['close']):
-            time.sleep(1)  # 等待 1 秒
+            time.sleep(0.5)
 
 # 关闭电磁阀
-close(True, True, True)  # 参数传True和False
+# open(True, False, False)  # 参数传True和False
+# close(False,False,True)
+# for i in range(10):
+#     open(False,True,True)
+#     close(True,True,True)

CU/Feeding.py

@@ -322,7 +322,8 @@ class Feeding(QObject):
             if feed_pos.get_position().compare(real_position):
                 self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
                 self.next_position(self.is_reverse)
-
+                if self.feedStatus == FeedStatus.FTake:
+                    self.catch.catch_status=CatchStatus.CTake
 
 
         elif self.feedStatus == FeedStatus.FPhoto:
@@ -444,17 +445,11 @@ class Feeding(QObject):
                     # self.sendIOControl(self.robotClient.con_ios[2], 1)
 
                     self.log_signal.emit(logging.INFO, "到达抓料点位")
-                    if self.catch.catch_status == CatchStatus.CNone :
+                    #self.feedConfig.feedLine.set_take_position(None)
-                        self.catch.catch_status = CatchStatus.CTake
+                    # time.sleep(self.robotClient.time_delay_take)
-                        return
+                    self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
-                    if self.catch.catch_status == CatchStatus.CTake:
+                    self.next_position()
-                        return
+                    self.catch.catch_status = CatchStatus.COk
-                    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)
-                        self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
-                        self.next_position()
 
             else:
                 self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)

Config/FeedLine.ini

@@ -20,12 +20,12 @@ status = 3
 linetype = 0
 
 [Position1]
-x = 7.0
+x = 1402.887451
-y = 50.0
+y = 1126.740479
-z = 1.0
+z = 2000.0
-u = 12.0
+u = 6.669909
-v = 0.0
+v = 7.899203
-w = 1.0
+w = -151.361526
 id = 1
 order = 4
 lineid = 1
@@ -33,12 +33,12 @@ status = 3
 linetype = 0
 
 [Position7]
-x = 7.0
+x = 230.52298
-y = 50.0
+y = 1879.445435
-z = 1.0
+z = 2000.432495
-u = 12.0
+u = 3.915686
-v = 0.0
+v = 18.44486
-w = 1.0
+w = -107.223564
 id = 7
 order = 0
 lineid = 1
@@ -46,64 +46,64 @@ status = 3
 linetype = 0
 
 [Position8]
-x = 7.0
+x = 1430.494385
-y = 50.0
+y = 1765.716187
-z = 1.0
+z = 2050.0
-u = 12.0
+u = 1.57722
-v = 0.0
+v = 4.174088
-w = 1.0
+w = -87.506218
 id = 8
-order = 9
+order = 8
 lineid = 1
 status = 6
 linetype = 0
 
 [Position9]
-x = 7.0
+x = 1430.492554
-y = 50.0
+y = 1765.717407
-z = 1.0
+z = 1832.536255
-u = 12.0
+u = 1.57702
-v = 0.0
+v = 4.174215
-w = 1.0
+w = -87.506783
 id = 9
-order = 10
+order = 9
 lineid = 1
 status = 7
 linetype = 0
 
 [Position10]
-x = 7.0
+x = 1375.01416
-y = 50.0
+y = 1702.021973
-z = 1.0
+z = 2117.369385
-u = 12.0
+u = 8.211453
-v = 0.0
+v = 4.232689
-w = 1.0
+w = -100.153625
 id = 10
-order = 11
+order = 10
 lineid = 1
 status = 8
 linetype = 0
 
 [Position12]
-x = 7.0
+x = 2180.736328
-y = 50.0
+y = 356.33316
-z = 1.0
+z = 1850.0
-u = 12.0
+u = 5.812903
-v = 0.0
+v = 5.431066
-w = 1.0
+w = -168.01712
 id = 12
-order = 12
+order = 11
 lineid = 1
 status = 9
 linetype = 0
 
 [Position2]
-x = 7.0
+x = 1765.864746
-y = 50.0
+y = 345.495361
-z = 1.0
+z = 2000.0
-u = 12.0
+u = 6.676573
-v = 0.0
+v = 7.941406
-w = 1.0
+w = -179.064972
 id = 2
 order = 3
 lineid = 1
@@ -124,12 +124,12 @@ status = 2
 linetype = 0
 
 [Position13]
-x = 7.0
+x = 1351.092285
-y = 50.0
+y = 1188.34668
-z = 1.0
+z = 2000.151001
-u = 12.0
+u = 6.676482
-v = 0.0
+v = 7.941721
-w = 1.0
+w = -148.802124
 id = 13
 order = 1
 lineid = 1
@@ -137,12 +137,12 @@ status = 3
 linetype = 0
 
 [Position14]
-x = 7.0
+x = 371.28418
-y = 50.0
+y = 1757.579224
-z = 1.0
+z = 2000.0
-u = 12.0
+u = 7.908469
-v = 0.0
+v = 6.585917
-w = 1.0
+w = -95.263153
 id = 14
 order = 5
 lineid = 1
@@ -150,12 +150,12 @@ status = 3
 linetype = 0
 
 [Position5]
-x = 7.0
+x = 352.000519
-y = 50.0
+y = 1915.820068
-z = 1.0
+z = 2022.926514
-u = 12.0
+u = -179.120407
-v = 0.0
+v = -1.876792
-w = 1.0
+w = 2.38191
 id = 5
 order = 6
 lineid = 1
@@ -163,41 +163,28 @@ status = 5
 linetype = 0
 
 [Position15]
-x = 7.0
+x = 1765.864746
-y = 50.0
+y = 345.495361
-z = 1.0
+z = 2000.0
-u = 12.0
+u = 6.676573
-v = 0.0
+v = 7.941406
-w = 1.0
+w = -179.064972
 id = 15
 order = 2
 lineid = 1
 status = 2
 linetype = 0
 
-[Position4]
+[Position16]
-x = 7.0
+x = 275.882446
-y = 50.0
+y = 2153.374023
-z = 1.0
+z = 2050.0
-u = 12.0
+u = 8.369349
-v = 0.0
+v = 1.971002
-w = 1.0
+w = -108.256897
-id = 4
+id = 16
 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
-

Constant.py

@@ -1,11 +1,11 @@
 import os
 
-Debug = True
+Debug = False
 IO_EmergencyPoint = 2
 max_log_len = 100
 bag_height = 10 # 一袋的高度
 position_accuracy_action = 0.1 #动作时的位置精度6  这个精度要高 必须到位置才做动作
-position_accuracy_command = 6#命令时的位置精度
+position_accuracy_command = 300 #命令时的位置精度
 manual_adjust_accuracy = 1
 # speed = 10
 # shake_speed = 20

Seting.ini

@@ -1,7 +1,7 @@
 [Main]
 
 [Robot_Feed]
-ipaddress = 127.0.0.1
+ipaddress = 192.168.20.4
 port = 502
 j1_min = -150
 j1_max = +150
@@ -47,7 +47,7 @@ photo_v5 = 0.0
 photo_w5 = 1.0
 linecount = 2
 remain_linename = 1
-remain_count = 999
+remain_count = 0
 io_take_addr = 8
 io_zip_addr = 11
 io_shake_addr = 12
@@ -56,7 +56,7 @@ putdelay = 0.1
 shakedelay = 1.0
 max_angle_interval = 20
 smooth = 9
-dynamic_height = 520.0
+dynamic_height = 350.0
 
 [Speed]
 debug_speed = 50
@@ -64,12 +64,12 @@ feed_speed = 550
 reset_speed = 35
 
 [Origin]
-x = 7.0
+x = 204.996765
-y = 50.0
+y = 1455.630493
-z = 1.0
+z = 2324.525146
-u = 12.0
+u = 6.460966
-v = 0.0
+v = 37.462826
-w = 1.0
+w = -84.569252
 
 [Camera_Feed]
 ipaddress = 127.0.0.1

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 = -30#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
+    delta = -200#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
     angle = np.asarray([a,b,c])
     noraml = camera2robot[:3, :3]@angle
     normal_vector = noraml / np.linalg.norm(noraml)

main.py

@@ -63,6 +63,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
         self.init_Run()
         self.init_robot_info()
 
+
         self.init_IOPanel()
         self.start_Runing()
         self.init_log()