def setDeviation():
global get_d
while True:
if get_d:
get_d = False
LeArm.setServo(1, 700, 500)
kin.ki_move(n_x, n_y, 750.0, 500)
else:
time.sleep(0.01)
def Arm_Pos_Corr():
LeArm.setServo(1, 1200, 500)
time.sleep(0.5)
kin.ki_move(0, 2250, 200.0, 1500)
def grab_blocks():
global sec_3_flag, no_move_last_x, x_pix_cm, step
global position_color_list, y_pix_cm
if sec_3_flag:
if -10 < x_pix_cm - no_move_last_x < 10:
while True:
if step == 0:
xx = position_color_list[0][0]
yy = position_color_list[0][1]
angle = position_color_list[0][2]
# 数据映射
n_xx = int(leMap(xx, 0.0, 320.0, -1250.0, 1250.0)) * 1.0
n_yy = int(leMap(240 - yy, 0.0, 240.0, 1250, 3250.0)) * 1.0
# 需要根据实际情况调整，偏差主要来自舵机的虚位
if n_xx < -100:
n_xx -= 120 # 偏差
LeArm.setServo(1, 700, 500)
time.sleep(0.5)
step = 1
elif step == 1:
# 机械臂下去
if kin.ki_move(n_xx, n_yy, 200.0, 1500):
step = 2
else:
step = 6
elif step == 2:
# 根据方块的角度转动爪子
if angle <= -45:
angle = -(90 + angle)
n_angle = leMap(angle, 0.0, -45.0, 1500.0, 1750.0)
if n_xx > 0:
LeArm.setServo(2, 3000 - n_angle, 500)
else:
LeArm.setServo(2, n_angle, 500)
time.sleep(0.5)
step = 3
elif step == 3:
# 抓取
print '3 ok'
LeArm.setServo(1, 1200, 500)
time.sleep(0.5)
step = 4
elif step == 4: # 将方块提起
print '4 ok'
kin.ki_move(n_xx, n_yy, 700.0, 1000)
step = 5
elif step == 5:
print '5 ok'
LeArm.runActionGroup('green', 1)
step = 6
elif step == 6:
print '6 ok'
LeArm.setServo(1, 700, 500)
time.sleep(0.5)
LeArm.runActionGroup('rest', 1)
position_color_list = []
sec_3_flag = False
step = 0
x_pix_cm = 0
y_pix_cm = 0
break
no_move_last_x = x_pix_cm
else:
time.sleep(0.01)
# 进入下一次定时器
threading.Timer(0.5, grab_blocks).start()
# 绘制中心点
cv2.circle(frame, (int(c_x), int(c_y)), 3,
(216, 0, 255), -1)
x_pix_cm = int(c_x)
y_pix_cm = int(c_y)
cv_blocks_ok = True # 开始搬运
if cv_blocks_ok and sec_3_flag is False:
# 数据映射
n_x = int(leMap(x_pix_cm, 0.0, 320.0, -1250.0, 1250.0)) * 1.0
n_y = int(leMap(240 - y_pix_cm, 0.0, 240.0, 1250,
3250.0)) * 1.0
# 需要根据实际情况调整，偏差主要来自舵机的虚位
if n_x < -100:
n_x -= 120 # 偏差
LeArm.setServo(1, 700, 500)
kin.ki_move(n_x, n_y, 750.0, 500)
cv_blocks_ok = False
if -20 < last_x - n_x < 20: # 判断色块是否已经不动了
no_move_count += 1
if no_move_count >= 4:
no_move_count = 0
sec_3_flag = True # 是就开启定时器
position_color_list.append(
(x_pix_cm, y_pix_cm, int(c_angle)))
else:
no_move_count = 0
last_x = n_x
# 画图像中心点
cv2.line(frame, ((img_w / 2) - 20, (img_h / 2)),
((img_w / 2) + 20, (img_h / 2)), (0, 0, 255), 1)
cv2.line(frame, ((img_w / 2), (img_h / 2) - 20),
def move_blocks():
global cv_blocks_ok, position_color_list, step
while True:
if cv_blocks_ok is True:
if len(position_color_list) == 1: #
# 数据处理
print(position_color_list, 'pos')
if step == 0:
x_pix_cm = position_color_list[0][1]
y_pix_cm = position_color_list[0][2]
angle = position_color_list[0][3]
# 数据映射
n_x = int(leMap(x_pix_cm, 0.0, 320.0, -1250.0,
1250.0)) * 1.0
n_y = int(leMap(240 - y_pix_cm, 0.0, 240.0, 1250,
3250.0)) * 1.0
# 需要根据实际情况调整，偏差主要来自舵机的虚位
if n_x < -100:
n_x -= 120 # 偏差
LeArm.setServo(1, 700, 500)
time.sleep(0.5)
step = 1
elif step == 1:
# 机械臂下去
if kin.ki_move(n_x, n_y, 200.0, 1500):
step = 2
else:
step = 6
elif step == 2:
# 根据方块的角度转动爪子
if angle <= -45:
angle = -(90 + angle)
n_angle = leMap(angle, 0.0, -45.0, 1500.0, 1750.0)
if n_x > 0:
LeArm.setServo(2, 3000 - n_angle, 500)
else:
LeArm.setServo(2, n_angle, 500)
time.sleep(0.5)
step = 3
elif step == 3:
# 抓取
print('3 ok')
LeArm.setServo(1, 1200, 500)
time.sleep(0.5)
step = 4
elif step == 4: # 将方块提起
print('4 ok')
kin.ki_move(n_x, n_y, 700.0, 1000)
step = 5
elif step == 5: # 抓取成功，放置方块
print('5 ok')
if position_color_list[0][0] == 'red':
LeArm.runActionGroup('red', 1)
elif position_color_list[0][0] == 'blue':
LeArm.runActionGroup('blue', 1)
elif position_color_list[0][0] == 'green':
LeArm.runActionGroup('green', 1)
step = 6
elif step == 6: # 复位机械臂
print('6 ok')
LeArm.runActionGroup('rest', 1)
threadLock.acquire()
position_color_list = []
cv_blocks_ok = False
threadLock.release()
step = 0
else:
time.sleep(0.01)
def move_blocks():
global cv_blocks_ok, position_mask_list, step
while True:
if cv_blocks_ok is True:
if len(position_mask_list) == 1:
print(position_mask_list, 'pos')
if step == 0:
x_pix_cm = position_mask_list[0][1]
y_pix_cm = position_mask_list[0][2]
angle = position_mask_list[0][3]
n_x = int(leMap(x_pix_cm, 0.0, 320.0, -1250.0,
1250.0)) * 1.0
n_y = int(leMap(240 - y_pix_cm, 0.0, 240.0, 1250,
3250.0)) * 1.0
if n_x < -100:
n_x -= 120
LeArm.setServo(1, 700, 500)
time.sleep(0.5)
step = 1
elif step == 1:
if kin.ki_move(n_x, n_y, 200.0, 1500):
step = 2
else:
step = 6
elif step == 2:
if angle <= -45:
angle = -(90 + angle)
n_angle = leMap(angle, 0.0, -45.0, 1500.0, 1750.0)
if n_x > 0:
LeArm.setServo(2, 3000 - n_angle, 500)
else:
LeArm.setServo(2, n_angle, 500)
time.sleep(0.5)
step = 3
elif step == 3:
print('3 ok')
LeArm.setServo(1, 1200, 500)
time.sleep(0.5)
step = 4
elif step == 4:
print('4 ok')
kin.ki_move(n_x, n_y, 700.0, 1000)
step = 5
elif step == 5:
print('5 ok')
if position_mask_list[0][0] == 'red':
LeArm.runActionGroup('red', 1)
elif position_mask_list[0][0] == 'blue':
LeArm.runActionGroup('blue', 1)
elif position_mask_list[0][0] == 'green':
LeArm.runActionGroup('green', 1)
step = 6
elif step == 6:
print('6 ok')
LeArm.runActionGroup('rest', 1)
threadLock.acquire()
position_mask_list = []
cv_blocks_ok = False
threadLock.release()
step = 0
else:
time.sleep(0.01)
import os
import sys
import time
sys.path.append(os.path.dirname(__file__) + "../armpi")
import kinematics as kin
import LeArm
class Gripper:
def __init__(self, id, open_pos=700):
self.id = id
self.open_pos = open_pos
def open(self):
LeArm.setServo(self.id, self.open_pos, 500)
def close(self, pos=1500):
LeArm.setServo(self.id, pos, 500)
if __name__ == '__main__':
gripper = Gripper(1)
gripper.open()
kin.ki_move(1500, 1000, 300, 2000)
time.sleep(1)
gripper.close()
time.sleep(1)
kin.ki_move(0, 2000, 200, 2000)