return dst
# 机械臂位置校准
def Arm_Pos_Corr():
LeArm.setServo(1, 1200, 500)
time.sleep(0.5)
kin.ki_move(0, 2250, 200.0, 1500)
# 运行程序前按下KEY2,进入校准机械臂位置， 校准完成后，再按下KEY退出
run_corr_one = 0
cv_continue(1, 1)
# 初始化机械臂位置
#down.main()
LeArm.runActionGroup('rest', 1)
LeArm.setServo(1, 700, 500)
while True:
if GPIO.input(key) == 0:
time.sleep(0.1)
if GPIO.input(key) == 0:
correction_flag = not correction_flag
if correction_flag is False:
LeArm.runActionGroup('rest', 1)
if correction_flag is False:
run_corr_one = 0
if minFrame is not None and get_image_ok:
t1 = cv2.getTickCount()
frame = lens_distortion_adjustment(minFrame)
img_h, img_w = frame.shape[:2]
# 获取图像中心点坐标x, y
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, -600.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, 1500, 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] == 'white':
#pass
LeArm.runActionGroup('red', 1)
step = 6
elif step == 6: # 复位机械臂
print '6 ok'
LeArm.runActionGroup('rest', 1)
LeArm.setServo(1, 700, 500)
time.sleep(0.5)
threadLock.acquire()
position_color_list = []
cv_blocks_ok = False
threadLock.release()
step = 0
else:
time.sleep(0.01)
import LeArm import kinematics as kin import sys LeArm.runActionGroup(str(sys.argv[1]), 1) print(str(sys.argv[1]))
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()
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)
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]
# 数据映射-1250 1250
n_x = int(leMap(x_pix_cm, 0.0, 320.0, -2050.0, 2050.0)) * 1.0
n_y = int(leMap(240 - y_pix_cm, 0.0, 240.0, 1050, 3550.0)) * 1.0 #1250 3250
print("n_x,n_y:",n_x,n_y)
# 需要根据实际情况调整，偏差主要来自舵机的虚位
#if n_x < -100:
# n_x -= 120 # 偏差
#if n_x < -500:
#n_x -= 120
if n_x < -1250:
n_x = 1250
if n_x > 1250:
n_x =1250
if n_y > 1600:
n_y += 300
#if n_y > 2400:
#n_y +=300
if n_y < 1250:
n_y = 1250
if n_y > 3250:
n_y = 3250
print("Adjusted n_x,n_y",n_x,n_y)
LeArm.setServo(1, 700, 500)
time.sleep(1.5)
step = 1
elif step == 1:
# 机械臂下去
if kin.ki_move(n_x, n_y, 900.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, 1510, 500)
time.sleep(1.5)
step = 4
elif step == 4: # 将方块提起
print '4 ok'
kin.ki_move(n_x, n_y, 1000.0, 1000)
step = 5
elif step == 5: # 抓取成功，放置方块
print '5 ok'
if position_color_list[0][0] == 'red':
LeArm.runActionGroup('hold', 1)
#elif position_color_list[0][0] == 'blue':
#LeArm.runActionGroup('blue', 1)
elif position_color_list[0][0] == 'green':
LeArm.runActionGroup('hold', 1)
#step = 6
#sys.exit()
#raise Exception('I know Python!')
#LeArm.setServo(7,600,500)
#up.main()
#time.sleep(0.5)
os._exit(1)
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_color_list, step
global stack_b_num
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
# 2、机械臂下去抓取
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 stack_b_num == 0:
LeArm.runActionGroup('stack_1', 1)
elif stack_b_num == 1:
LeArm.runActionGroup('stack_2', 1)
elif stack_b_num == 2:
LeArm.runActionGroup('stack_3', 1)
stack_b_num += 1
if stack_b_num == 3:
stack_b_num = 0
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)