class set_arm_control(): def __init__(self,RELAY_PIN=12): self.arm = SwiftAPI() self.arm.get_power_status() self.RELAY_PIN = RELAY_PIN self.arm.reset() #機械手臂進行初始化動作 def set_arm_origin(self): self.arm.waiting_ready() self.arm.set_position(200, 0, 170, speed=100000) # 機械手臂設定(x, y, z)座標移動 def set_arm_move(self, x, y, z): self.arm.set_position(x, y, z, speed=100000) # 機械手臂切斷連線 def arm_disconnect(self): self.arm.disconnect() def get_position(self): print(self.arm.get_position()) #初始設定繼電器 def origin_pump(self): GPIO.setmode(GPIO.BOARD) # 指定模式BOARD 啟用RPi板子相對應之腳位編號 GPIO.setwarnings(False) # 避免出現警告 GPIO.setup(self.RELAY_PIN, GPIO.OUT) # 設定pin腳為輸出 #打開幫浦=開啟繼電器(使電流流通) def start_pump(self): GPIO.output(self.RELAY_PIN, 0) #關閉幫浦=關閉繼電器(使電流不能流通) def close_pump(self): GPIO.output(self.RELAY_PIN, 1)
from uarm.wrapper import SwiftAPI swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) swift.waiting_ready(timeout=3) device_info = swift.get_device_info() print(device_info) firmware_version = device_info['firmware_version'] if firmware_version and not firmware_version.startswith( ('0.', '1.', '2.', '3.')): swift.set_speed_factor(0.0005) swift.set_mode(0) swift.reset(wait=True, speed=10000) swift.set_position(x=200, speed=10000 * 20) swift.set_position(y=100) swift.set_position(z=100) swift.flush_cmd(wait_stop=True) swift.set_polar(stretch=200, speed=10000 * 20) swift.set_polar(rotation=90) swift.set_polar(height=150) print(swift.set_polar(stretch=200, rotation=90, height=150, wait=True)) swift.flush_cmd() # time.sleep(1) # print('mv 1') # swift.set_position(x=0, y=0, z=0, speed=10000*20)
device_info = swift.get_device_info() print(swift.port, device_info) firmware_version = device_info['firmware_version'] if firmware_version and not firmware_version.startswith(('0.', '1.', '2.', '3.')): swift.set_speed_factor(0.00001) swift.set_servo_detach() input('A4') swift.set_servo_attach() pos = swift.get_position() print(pos); bias = 23; swift.reset(speed=10000000); seqs = ["F", "G", "E", "A", "D", "G", "C", "C"] # seqs = ["C", "C", "D", "D", "E", "E", "D", "D"] count = 0; while True: count = count + 1 seq = seqs[count % len(seqs)] print(seq); print(time.time()) # move swift.set_position(x=pos[X], y=pos[Y] + (-d[seq] * bias), z=pos[Z] + 5, speed=10000000) # push
from uarm.wrapper import SwiftAPI sio = socketio.AsyncClient(reconnection=True) try: swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, callback_thread_pool_size=2) swift.waiting_ready(timeout=10) device_info = swift.get_device_info() print(device_info) # swift.set_speed_factor(100) swift.set_mode(3) swift.reset(wait=True, speed=250) # firmware_version = device_info['firmware_version'] # if firmware_version and not firmware_version.startswith(('0.', '1.', '2.', '3.')): # # swift.set_speed_factor(0.0005) except Exception: swift = False print('swift uArm not connected') @sio.event async def connect(): print('connected to server') @sio.event
import os import sys import time sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from uarm.wrapper import SwiftAPI swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) swift.waiting_ready(timeout=3) device_info = swift.get_device_info() print(device_info) firmware_version = device_info['firmware_version'] #if firmware_version and not firmware_version.startswith(('0.', '1.', '2.', '3.')): # swift.set_speed_factor(0.0005) #set for general mode swift.set_mode(0) ''' swift.reset(wait=True, speed=100000) swift.set_position(x=200, speed=100000) swift.set_position(y=100) swift.set_position(z=100) swift.flush_cmd(wait_stop=True) ''' ''' swift.set_polar(stretch=150, speed=100000) swift.set_polar(rotation=0) swift.set_polar(height=80) print(swift.set_polar(stretch=150, rotation=0, height=80, wait=True)) swift.flush_cmd() ''' ''' swift.set_polar(stretch=150, rotation=90, height=150, speed=100000, wait=True)
class UArm_SDK(object): def __init__(self): ''' connect to UArm ''' self.swift = SwiftAPI() self.swift.connect() self.swift.get_power_status() print(self.swift.get_device_info()) self.swift.reset(wait=True) # back to home position print('init complete') self.gripper_temp = 0 # keep track of gripper state def __del__(self): ''' disconnect UArm ''' self.swift.disconnect() print('uarm disconnected') def set_servo_angle(self, joint_angles, dt): ''' set servo angle via SDK input: joint_angles, 5-vector: [theta1, theta2, theta3, theta4, pump state] in degrees dt, time step ''' wait = True self.swift.set_servo_angle(servo_id=0, angle=joint_angles[0] + 90, speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=1, angle=joint_angles[1], speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=2, angle=joint_angles[2] - joint_angles[1], speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=3, angle=180 - joint_angles[3], speed=5000, wait=wait) time.sleep(dt / 4) if joint_angles[4] > 0: self.swift.set_pump(on=True) elif joint_angles[4] == 0: self.swift.set_pump(on=False) else: print("ERROR") def control_uarm_via_traj(self, position, wrist_angle, pump_state, dt): ''' set end effector position, wrist angle and pump state via SDK input: position, 3-vector: [px, py, pz] wrist_angle: wrist angle in rad pump_state: bool, 0 - off, 1 - on ''' px, py, pz = position[0], position[1], position[2] # conver m to mm px *= 1000 py *= 1000 pz *= 1000 # change end effector position e = self.swift.set_position(x=px, y=py, z=pz, speed=100000, wait=True) print(e) # change wrist angle self.swift.set_wrist(90 - wrist_angle * 180 / PI) if self.gripper_temp == 0 and pump_state == 1: # enable suction cup self.swift.set_pump(on=True, wait=True) print('pump on') self.gripper_temp = 1 if self.gripper_temp == 1 and pump_state == 0: # disable suction cup self.swift.set_pump(on=False, wait=True) print('pump off') self.gripper_temp = 0 time.sleep(dt)
import sys import time import functools sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from uarm.wrapper import SwiftAPI from uarm.utils.log import logger logger.setLevel(logger.DEBUG) swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_handle_thread=False) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_write_thread=True) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_report_thread=True) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_handle_thread=True, enable_write_thread=True, enable_report_thread=True) swift.reset(speed=50000, wait=True) swift.set_position(x=250, y=0, z=10, speed=50000, wait=True) swift.flush_cmd() swift.set_pump(True) swift.set_position(x=250, y=0, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=220, y=70, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=200, y=140, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=180, y=210, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=250, y=210, z=10, speed=50000, wait=True) swift.flush_cmd() swift.set_pump(False) swift.flush_cmd()
swift.get_power_status() print(swift.get_device_info()) T = 2 dt = 0.01 # best we can get: 0.01 N = int(T / dt) #swift.set_servo_angle(servo_id=0, angle=90) ##time.sleep(3) #swift.set_servo_angle(servo_id=1, angle=90) ##time.sleep(3) #swift.set_servo_angle(servo_id=2, angle=0) ##time.sleep(3) #swift.set_servo_angle(servo_id=3, angle=90) swift.reset() time.sleep(3) swift.set_speed_factor(2) '''generate configs''' joint_angles = np.zeros((N, 5)) for i in range(N): joint_angles[i, :] = np.array( [90, 90 - 90 / T * dt * i, 90 / T * dt * i, 90, 0]) # step motor step? for i in range(N): swift.set_servo_angle(servo_id=0, angle=joint_angles[i, 0]) # time.sleep(dt/4) swift.set_servo_angle(servo_id=1, angle=joint_angles[i, 1])
class Arm: def __init__(self): self.arm = SwiftAPI() self.arm.reset() sleep(2) def rest(self): self.arm.set_position(x=0, y=180, z=146, wait=True, timeout=10, speed=10000) print("moving to rest pos!") def correction(self): print("=================check start!=================\n") self.arm.set_position(x=227, y=0, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=227, y=0, z=47, wait=True, timeout=10, speed=10000) print("check center\n") sleep(1) input() self.arm.set_position(x=292, y=75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=292, y=75, z=47, wait=True, timeout=10, speed=10000) print("check left-up\n") sleep(1) input() self.arm.set_position(x=293, y=-75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=293, y=-75, z=47, wait=True, timeout=10, speed=10000) print("check right-up\n") sleep(1) input() self.arm.set_position(x=160, y=75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=160, y=75, z=47, wait=True, timeout=10, speed=10000) print("check left-down\n") sleep(1) input() self.arm.set_position(x=160, y=-75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=160, y=-75, z=47, wait=True, timeout=10, speed=10000) print("check right-down\n") sleep(1) input() self.arm.set_position(x=0, y=180, z=146, wait=True, timeout=10, speed=10000) print("=================check finish!=================\n") def move(self, color, coords): global count count += 1 if coords == "pass" or coords == "resign": return None y, x = coords y = int(ord(y) - ord('A')) x = int(x) - 1 if count == 1: x_take = 309 elif count == 2: x_take = 280 elif count == 3: x_take = 254 elif count == 4: x_take = 223 elif count == 5: x_take = 200 elif count == 6: x_take = 170 elif count == 7: x_take = 144 count = 0 if color == "B": xp = x_take yp = -145 zp = 100 elif color == "W": xp = x_take yp = 157 zp = 100 else: print("Wrong color input!") self.arm.set_position(x=xp, y=yp, z=zp, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) self.arm.set_position(z=54, wait=True, speed=10000) sleep(1.5) self.arm.set_pump(True) sleep(2) self.arm.set_position(z=100, wait=True, speed=10000) self.arm.set_position(x=294 - x * 22, y=-66 + y * 24, z=100, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) print("moving to:", coords) self.arm.set_position(z=54, wait=True, speed=10000) # original z is -26 sleep(1) self.arm.set_pump(False) sleep(0.5) self.arm.set_position(z=100, wait=True, speed=10000) sleep(0.5) self.arm.flush_cmd() def remove(self, coords, remove_list): self.arm.set_position(x=1, y=2, z=45, wait=True, speed=10000) x, y = coords remove_list def remove_chess(self, color, coords): y, x = coords y = int(y) - 1 x = int(x) - 1 self.arm.set_position(x=294 - x * 22, y=-66 + y * 24, z=100, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) self.arm.set_position(z=54, wait=True, speed=10000) sleep(1.5) self.arm.set_pump(True) sleep(2) print("removing :", coords) self.arm.set_position(z=100, wait=True, speed=10000) self.arm.set_position(x=342, y=0, z=90, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) self.arm.set_pump(False) sleep(0.5) def take_photo(self): self.arm.set_position(x=166, y=4, z=220, wait=True, timeout=10, speed=10000) print("Take Picture place!") def game_over(self): self.arm.set_position(x=0, y=170, z=100, wait=True, timeout=10, speed=10000) print("Game over!")
import os import sys import time sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from uarm.wrapper import SwiftAPI swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) swift.waiting_ready(timeout=3) device_info = swift.get_device_info() print(device_info) firmware_version = device_info['firmware_version'] if firmware_version and not firmware_version.startswith( ('0.', '1.', '2.', '3.')): swift.set_speed_factor(0.0005) swift.set_mode(0) ''' swift.reset(wait=True, speed=100000) swift.set_position(x=200, speed=100000) swift.set_position(y=100) swift.set_position(z=100) swift.flush_cmd(wait_stop=True) ''' #set_digital_output(pin=None, value=None, wait=True, timeout=None, callback=None) swift.set_digital_output(pin=50, value=1) # red led swift.set_digital_output(pin=49, value=0) # green led swift.flush_cmd() i = 0 while i < 5: swift.set_polar(stretch=150, speed=100000)
class MinionArm: def __init__(self): self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.swift.waiting_ready() device_info = self.swift.get_device_info() print(device_info) firmware_version = device_info['firmware_version'] if firmware_version and not firmware_version.startswith( ('0.', '1.', '2.', '3.')): self.swift.set_speed_factor(0.0005) self.speed = 110000 self.test() self.board = "right" def test(self): # self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True) self.swift.reset(wait=True, speed=self.speed) self.move(1250, 0, 150, self.speed) print(self.swift.get_position()) # self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True) def home(self): print("going home") self.swift.set_position(x=150, y=0, speed=self.speed * 0.5, wait=True) self.move(145, 0, 100, self.speed * 1.5) time.sleep(2) # self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True) def rest(self): print("going to rest position") self.swift.set_position(x=150, speed=self.speed * 0.5, wait=True) self.swift.set_position(x=125, y=0, speed=self.speed * 0.5, wait=True) self.move(125, 0, 40, self.speed * 0.5) time.sleep(2) self.swift.set_buzzer(frequency=1000, duration=0.5) def move(self, x, y, z, speed): self.swift.set_position(x, y, z, speed, wait=True) def flip(self): print("flipping") # self.home() self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True) if self.board == "right": self.flip_left() self.board = "left" else: self.flip_right() self.board = "right" self.home() self.swift.set_buzzer(frequency=500, duration=1.0, wait=True) def flip_left(self): self.move(180, 0, 150, self.speed) self.move(240, 0, 150, self.speed) # self.move(220, 10, 140, self.speed*.2) self.move(220, 40, 150, self.speed * .2) # self.move(220, 40, 110, self.speed*.2) # self.move(220, 60, 100, self.speed*.2) self.move(220, 50, 30, self.speed * .2) time.sleep(1) self.move(180, 50, 20, self.speed * .2) time.sleep(2) def flip_right(self): self.move(180, 0, 150, self.speed) self.move(240, -10, 150, self.speed) # self.move(220, -20, 140, self.speed*.2) self.move(220, -40, 150, self.speed * .2) # self.move(220, -50, 110, self.speed*.2) # self.move(220, -60, 100, self.speed*.2) self.move(220, -60, 20, self.speed * .2) self.move(180, -50, 20, self.speed * .2) time.sleep(2) def move_to_galton_cal_pos(self): self.move(180, 0, 150, self.speed) self.move(240, 0, 150, self.speed) wait = input("waiting") self.home() def disconnect(self): self.swift.disconnect()