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 ki_move(x, y, z, movetime):
alpha_list = []
for alp in range(-25, -65, -1):
if kinematic_analysis(x, y, z, alp):
alpha_list.append(alp)
if len(alpha_list) > 0:
# print 'y', y
if y > 2150:
best_alpha = max(alpha_list)
else:
best_alpha = min(alpha_list)
# print best_alpha
theta3, theta4, theta5, theta6 = kinematic_analysis(
x, y, z, best_alpha)
# print theta3, theta4, theta5, theta6
pwm_6 = int(2000.0 * theta6 / 180.0 + 500.0)
pwm_5 = int(2000.0 * (90.0 - theta5) / 180.0 + 1500.0)
pwm_4 = int(2000.0 * (135.0 - theta4) / 270.0 + 500.0)
pwm_3 = int(2000.0 * theta3 / 180.0 + 1500.0)
# print 'pwm', pwm_3, pwm_4, pwm_5, pwm_6
LeArm.setServo(3, pwm_3, movetime)
LeArm.setServo(4, pwm_4, movetime)
LeArm.setServo(5, pwm_5, movetime)
LeArm.setServo(6, pwm_6, movetime)
time.sleep(movetime / 1000.0)
return True
else:
return False
def cmd_i001(par): # 指令001 舵机运动
global Deviation
if par[0] > 30000:
par[0] = 30000
if par[0] < 20:
par[0] = 20
if not par[1] * 2 + 2 == len(par) or not len(par) >= 4:
raise LeError(tuple(par), "舵机运动指令长度错误")
Servos = par[2:]
for i in range(0, len(Servos), 2):
if Servos[i] > 6 or Servos[i] < 1 or Servos[i + 1] > 2500 or Servos[
i + 1] < 500:
raise LeError((Servos[i], Servos[i + 1]), "舵机运动参数错误")
LeArm.setServo(Servos[i], Servos[i + 1], par[0])
def move(x, y, z, target_alpha, movetime):
if kinematic_analysis(x, y, z, target_alpha):
print('OK')
theta3, theta4, theta5, theta6 = kinematic_analysis(
x, y, z, target_alpha)
pwm_6 = int(2000.0 * theta6 / 180.0 + 500.0)
pwm_5 = int(2000.0 * (90.0 - theta5) / 180.0 + 1500.0)
pwm_4 = int(2000.0 * (135.0 - theta4) / 270.0 + 500.0)
pwm_3 = int(2000.0 * theta3 / 180.0 + 1500.0)
LeArm.setServo(3, pwm_3, movetime)
LeArm.setServo(4, pwm_4, movetime)
LeArm.setServo(5, pwm_5, movetime)
LeArm.setServo(6, pwm_6, movetime)
time.sleep(movetime / 1000.0)
return True
else:
print('NG')
return False
def ki_move(x, y, z, movetime):#x，y，z为给定坐标，movetime为舵机转动时间
alpha_list = []
for alp in range(-25, -65, -1):#遍历爪子与水平面的夹角，在-25到-65求解，其他范围夹取物体效果不好
if kinematic_analysis(x, y, z, alp):
alpha_list.append(alp)
if len(alpha_list) > 0:
if y > 2150:
best_alpha = max(alpha_list)
else:
best_alpha = min(alpha_list)
theta3, theta4, theta5, theta6 = kinematic_analysis(x, y, z, best_alpha)
pwm_6 = int(2000.0 * theta6 / 180.0 + 500.0)
pwm_5 = int(2000.0 * (90.0 - theta5) / 180.0 + 1500.0)
pwm_4 = int(2000.0 * (135.0 - theta4) / 270.0 + 500.0)
pwm_3 = int(2000.0 * theta3 / 180.0 + 1500.0)
LeArm.setServo(3, pwm_3, movetime)
LeArm.setServo(4, pwm_4, movetime)
LeArm.setServo(5, pwm_5, movetime)
LeArm.setServo(6, pwm_6, movetime)
time.sleep(movetime / 1000.0)
return True
else:
return False
# 机械臂位置校准
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
img_center_x = img_w / 2
import cv2 import numpy as np import time import urllib import threading import signal import LeArm import kinematics_apple as kin import RPi.GPIO as GPIO LeArm.setServo(1,800,500) time.sleep(1.5)
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()
new_pwm_3 = int(motor_data[2])
new_pwm_4 = int(motor_data[3])
new_pwm_5 = int(motor_data[4])
new_pwm_6 = int(motor_data[5])
# Data lock.
if new_pwm_1 >= 500 and new_pwm_1 <= 2500:
pwm_1 = new_pwm_1
if new_pwm_2 >= 1000 and new_pwm_2 <= 2000:
pwm_2 = new_pwm_2
if new_pwm_3 >= 500 and new_pwm_3 <= 2500:
pwm_3 = new_pwm_3
# Limit the MIDDLE ARM.
if new_pwm_4 >= 1500 and new_pwm_4 <= 2500:
pwm_4 = new_pwm_4
# Limit ELBOW SWING.
if new_pwm_5 >= 1500 and new_pwm_5 <= 2400:
pwm_5 = new_pwm_5
if new_pwm_6 >= 1000 and new_pwm_6 <= 2000:
pwm_6 = new_pwm_6
except:
pass
# Execute the motion.
LeArm.setServo(1, pwm_1, movetime) # CLAMP
LeArm.setServo(2, pwm_2, movetime) # WRIST ROTATE
LeArm.setServo(3, pwm_3, movetime) # WRIST SWING
LeArm.setServo(4, pwm_4, movetime) # MIDDLE ARM
LeArm.setServo(5, pwm_5, movetime) # ELBOW SWING
LeArm.setServo(6, pwm_6, movetime) # ELBOW ROTATE
time.sleep(movetime/1000 + 0.001)
def main():
time.sleep(0.5)
LeArm.setServo(7, 500, 1500)
time.sleep(2)
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 close(self, pos=1500):
LeArm.setServo(self.id, pos, 500)
def open(self):
LeArm.setServo(self.id, self.open_pos, 500)
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)
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)
def Arm_Pos_Corr():
LeArm.setServo(1, 1200, 500)
time.sleep(0.5)
kin.ki_move(0, 2250, 200.0, 1500)
def main():
time.sleep(1)
LeArm.setServo(7, 2200, 1500)
time.sleep(2)