kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

update

Browse source
This commit is contained in:
2929ss 2022-07-22 11:46:27 +08:00
parent ece15a8011
commit 52540c311d
2 changed files with 26 additions and 23 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

32
mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py
View file

@ -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)

17
mycobot_ai/ai_mecharm_270/scripts/test.py
View file

@ -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))