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 3339011..8c14251 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 @@ -20,13 +20,14 @@ __version__ = "1.0" class Object_detect(Movement): - def __init__(self, camera_x = 140, camera_y = -5): + def __init__(self, camera_x = 140, camera_y = 5): # m5 + # def __init__(self, camera_x = 140, camera_y = -5): # pi # inherit the parent class super(Object_detect, self).__init__() # get path of file dir_path = os.path.dirname(__file__) - # declare mypal260 + # declare 270 self.mc = None # 移动角度 @@ -82,11 +83,11 @@ class Object_detect(Movement): "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])], } - # use to calculate coord between cube and mypal260 + # use to calculate coord between cube and 270 self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 - # The coordinates of the grab center point relative to the mypal260 + # The coordinates of the grab center point relative to the 270 self.camera_x, self.camera_y = camera_x, camera_y - # The coordinates of the cube relative to the mypal260 + # The coordinates of the cube relative to the 270 self.c_x, self.c_y = 0, 0 # The ratio of pixels to actual values self.ratio = 0 @@ -158,12 +159,14 @@ 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.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 + 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) # open pump @@ -186,6 +189,7 @@ class Object_detect(Movement): time.sleep(3) + 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, @@ -221,7 +225,7 @@ class Object_detect(Movement): # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 self.move(x, y, color) - # init mypal260 + # init 270 def run(self): if "dev" in self.robot_m5: self.mc = MyCobot(self.robot_m5, 115200) @@ -283,13 +287,13 @@ class Object_detect(Movement): self.y2 = int(y2) print(self.x1, self.y1, self.x2, self.y2) - # set parameters to calculate the coords between cube and mypal260 + # set parameters to calculate the coords between cube and 270 def set_params(self, c_x, c_y, ratio): self.c_x = c_x self.c_y = c_y self.ratio = 220.0/ratio - # calculate the coords between cube and mypal260 + # calculate the coords between cube and 270 def get_position(self, x, y): return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y) @@ -363,7 +367,7 @@ class Object_detect(Movement): cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2) # calculate the rectangle center x, y = (x*2+w)/2, (y*2+h)/2 - # calculate the real coordinates of mypal260 relative to the target + # calculate the real coordinates of 270 relative to the target if mycolor == "red": self.color = 0 @@ -388,7 +392,7 @@ if __name__ == "__main__": cap.open() # init a class of Object_detect detect = Object_detect() - # init mypal260 + # init 270 detect.run() _init_ = 20 @@ -431,7 +435,7 @@ if __name__ == "__main__": init_num += 1 continue - # calculate params of the coords between cube and mypal260 + # calculate params of the coords between cube and 270 if nparams < 10: if detect.get_calculate_params(frame) is None: cv2.imshow("figure", frame) @@ -448,7 +452,7 @@ if __name__ == "__main__": continue elif nparams == 10: nparams += 1 - # calculate and set params of calculating real coord between cube and mypal260 + # calculate and set params of calculating real coord between cube and 270 detect.set_params( (detect.sum_x1+detect.sum_x2)/20.0, (detect.sum_y1+detect.sum_y2)/20.0, @@ -465,7 +469,7 @@ if __name__ == "__main__": continue else: x, y = detect_result - # calculate real coord between cube and mypal260 + # calculate real coord between cube and 270 real_x, real_y = detect.get_position(x, y) if num == 20: detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0) diff --git a/mycobot_ai/ai_mecharm_270/scripts/test.py b/mycobot_ai/ai_mecharm_270/scripts/test.py index d37cac0..ab7df7e 100755 --- a/mycobot_ai/ai_mecharm_270/scripts/test.py +++ b/mycobot_ai/ai_mecharm_270/scripts/test.py @@ -6,14 +6,15 @@ from pymycobot.genre import Angle from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化 import time,os -# mc = MyCobot(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) +mc = MyCobot(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) # mc = MyCobot(os.popen("ls /dev/ttyACM*").readline()[:-1], 115200) + # mc = MyCobot("/dev/ttyAMA0", 1000000) -mc = MyCobot(PI_PORT,PI_BAUD) +# 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) @@ -42,9 +43,9 @@ mc = MyCobot(PI_PORT,PI_BAUD) # print("\n") # mc.release_all_servos() -mc.release_servo(2) +# mc.release_servo(3) # mc.set_servo_calibration(1) -mc.set_servo_calibration(2) +# mc.set_servo_calibration(2) # mc.set_servo_calibration(3) # mc.set_servo_calibration(4) # mc.set_servo_calibration(5) @@ -53,11 +54,9 @@ mc.set_servo_calibration(2) # mc.set_basic_output(2, 0) # mc.set_basic_output(5, 0) -# time.sleep(3) - +# time.sleep(2) # mc.set_basic_output(2, 1) # mc.set_basic_output(5, 1) -# time.sleep(3) # print(mc.get_basic_input(2)) # print(mc.get_basic_input(5))