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 cmd_i007(sock, data=[]):
try:
time = int(data[0])
servo_num = int(data[1])
#print(time,servo_num)
servo_data = []
for i in range(servo_num):
servo_id = int(data[2 + i * 2])
servo_pos = int(data[3 + i * 2])
servo_data.append((servo_id, servo_pos - 10000))
#print(servo_data)
except:
raise LeError(tuple(data), "canshucuowu")
try:
for d in servo_data:
LeArm.setServo_CMP(d[0], d[1], time)
except Exception as e:
print(cuowu, e)
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 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)
@author: wqf
"""
from math import *
import LeArm
import time
import matplotlib.pyplot as plt
import LeConf
d = []
for i in range(0, len(LeConf.Deviation), 1):
if LeConf.Deviation[i] > 1600 or LeConf.Deviation < 1400:
print("out range 1200-1800")
else:
d.append(LeConf.Deviation[i] - 1500)
LeArm.initLeArm(tuple(d))
targetX = 0
targetY = 0
targetZ = 4000
lastX = 0
lastY = 0
lastZ = 4000
l0 = 960
l1 = 1050
l2 = 880
l3 = 1650
alpha = 0
# theta3 = 0
# theta4 = 0
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)
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()
class LeServer(SocketServer.ThreadingTCPServer):
allow_reuse_address = True
if __name__ == "__main__":
if not len(LeConf.Deviation) == 6:
print("偏差数量错误")
sys.exit()
else:
d = []
for i in range(0, len(LeConf.Deviation), 1):
if LeConf.Deviation[i] > 1600 or LeConf.Deviation < 1400:
print("偏差值超出范围1200-1800")
sys.exit()
else:
d.append(LeConf.Deviation[i] - 1500)
LeArm.initLeArm(tuple(d))
LeCmd.cmd_i001(
[1000, 6, 1, 1500, 2, 1500, 3, 1500, 4, 1500, 5, 1500, 6, 1500])
server = LeServer(("", 8947), ServoServer)
try:
server_thread = threading.Thread(target=server.serve_forever)
server_thread.start()
while True:
time.sleep(0.1)
except:
server.shutdown()
server.server_close()
LeArm.stopLeArm()
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)
th1.setDaemon(True) # 设置为后台线程，这里默认是False，设置为True之后则主线程不用等待子线程
th1.start()
# 机械臂位置校准
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)
# 初始化机械臂位置
LeArm.runActionGroup('rest', 1)
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 = minFrame
img_h, img_w = frame.shape[:2]
# 画图像中心点
cv2.line(frame, ((img_w / 2) - 20, (img_h / 2)),
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 open(self):
LeArm.setServo(self.id, self.open_pos, 500)
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)
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 cmd_i002(par): # 指令002 停止运动
LeArm.stopActionGroup()
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)