From f8d2b07193a6d2757f23ff62c8fbb620c45fed71 Mon Sep 17 00:00:00 2001
From: 2929ss <1814754095@qq.com>
Date: Mon, 25 Jul 2022 17:16:40 +0800
Subject: [PATCH] update

---
 .../scripts/combine_detect_obj_color.py       |  5 ++++-
 .../combine_detect_obj_img_folder_opt.py      | 20 ++++++++++---------
 mycobot_ai/ai_mecharm_270/scripts/test.py     |  6 +++---
 3 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py
index 8c14251..a1a39c3 100644
--- a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py
+++ b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py
@@ -159,15 +159,18 @@ class Object_detect(Movement):
         print(color)
         self.mc.send_angles(self.move_angles[0], 30)
         time.sleep(4)
-        # self.pub_marker(self.move_angles[0])
 
         # send coordinates to move 270
         self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
         time.sleep(3)
+        self.pub_marker(x/1000.0, y/1000.0, 140/1000.0)
+
         
         self.mc.send_coords([x, y, 95, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
         # self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
         time.sleep(3)
+        self.pub_marker(x/1000.0, y/1000.0, 90/1000.0)
+
         
         # open pump
         if "dev" in self.robot_m5:
diff --git a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py
index 23432f5..06cf6ce 100755
--- a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py
+++ b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py
@@ -178,14 +178,16 @@ class Object_detect(Movement):
     def move(self, x, y, color):
         # send Angle to move 270
         self.mc.send_angles(self.move_angles[0], 30)
-        time.sleep(4)
-        
+        time.sleep(7)
+
+        print("x %s ,y %s" % (x,y))
         # send coordinates to move 270 根据不同底板机械臂，调整吸泵高度
         self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
-        time.sleep(4)
-        
+        time.sleep(7)
+        print("ntm")
+
         self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
-        time.sleep(4)
+        time.sleep(6)
 
         # open pump
         if "dev" in self.robot_m5:
@@ -204,14 +206,14 @@ class Object_detect(Movement):
         
         # print(tmp)
         self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
-        time.sleep(3)
+        time.sleep(6)
         
 
         self.mc.send_coords(self.move_coords[color], 30, 1)
         self.pub_marker(self.move_coords[color][0] / 1000.0,
                         self.move_coords[color][1] / 1000.0,
                         self.move_coords[color][2] / 1000.0)
-        time.sleep(3)
+        time.sleep(6)
 
         # close pump
         if "dev" in self.robot_m5:
@@ -221,7 +223,7 @@ class Object_detect(Movement):
         time.sleep(6)
 
         self.mc.send_angles(self.move_angles[1], 30)
-        time.sleep(1.5)
+        time.sleep(6)
 
     # decide whether grab cube
     def decide_move(self, x, y, color):
@@ -431,7 +433,7 @@ class Object_detect(Movement):
 # The path to save the image folder
 def parse_folder(folder):
     restore = []
-    path = ''
+    # path = ''
     path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder
     path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder
 
diff --git a/mycobot_ai/ai_mecharm_270/scripts/test.py b/mycobot_ai/ai_mecharm_270/scripts/test.py
index ab7df7e..1ddc6da 100755
--- a/mycobot_ai/ai_mecharm_270/scripts/test.py
+++ b/mycobot_ai/ai_mecharm_270/scripts/test.py
@@ -13,8 +13,8 @@ mc = MyCobot(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200)
 # mc = MyCobot(PI_PORT,PI_BAUD)
 
 
-mc.send_angles([0,0,0,0,90,0],20) # coords:[95.6, 0.5, 166.4, 179.12, -0.18, 179.46]
-time.sleep(4)
+# mc.send_angles([0,0,0,0,90,0],20) # coords:[95.6, 0.5, 166.4, 179.12, -0.18, 179.46]
+# time.sleep(4)
 
 # mc.send_angles([-3.25, 17.22, -32.51, 2.37, 92.54, -36.21],30)
 
@@ -30,7 +30,7 @@ time.sleep(4)
 # mc.send_coords([-5.4, 120.6, 204.6, 162.66, -6.96, 159.93],30,0) # above the gray bucket
 # time.sleep(3)
 
-# mc.send_coords([80, 0, 130, 0 ,0, 0], 30, 0)
+mc.send_coords([80, 0, 92, 179.12, -0.18, 179.46], 30, 0)
 
 # mc.send_angle(3,0,25)
 # print(mc.get_angles())