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)