class uArmSwift: def __init__(self): self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, cmd_pend_size=2, callback_thread_pool_size=1) if not self.swift.connected: print('lose connect') 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.00005) self.swift.set_mode(0) self.speed = 500000 self.swift.set_wrist(angle=90) self.wristAngle = self.swift.get_servo_angle(0, timeout=10) def set_position(self, x=100, y=0, z=100, wait=False): self.swift.set_position(x, y, z, speed=self.speed, wait=wait) def set_polar(self, stretch, rotation, height, wait=False): self.swift.set_polar(stretch, rotation, height, speed=self.speed, wait=wait) def set_servo_angle(self, num, angle, wait=False): if num < 0 and num > 3: print("num is wrong") self.swift.set_servo_angle(num, angle, wait, speed=self.speed, wait=wait) def set_wrist(self, angle=90, wait=False): # 第四电机 self.swift.set_wrist(angle, wait) def set_pump(self, on=False): self.swift.set_pump(on) def set_buzzer(self, freq=1000, duration=1, wait=False): self.swift.set_buzzer(freq, duration, wait) def get_position(self): return self.swift.get_position() def get_servo_angle(self, id=0): return self.swift.get_servo_angle(id, timeout=10) def is_moving(self): return self.swift.get_is_moving() def disconnect(self): self.swift.disconnect()
from uarm.wrapper import SwiftAPI swift = SwiftAPI() swift.waiting_ready(timeout=10) swift.set_position(140, 0, 10, wait=True) swift.set_servo_detach() test1 = swift.get_position() print(test1) test2 = swift.get_servo_angle() print(test2) test = input( "now the uArm is detached, that mean you can freely move the arm without the servo-motors blocking it. for the next step, deplace the arm in another position than before and type enter to continue" ) test1 = swift.get_position() print(test1) test2 = swift.get_servo_angle() print(test2) test = input( "has you can see, the arm has moved but the position is the same : after detached the arm, if you move it you can only have the servo-motors angles. type enter to continue BUT be careful : the arm will be attach so don't block it in any way." ) swift.set_servo_attach() swift.set_position(140, 0, 50, wait=True)
y = data[0] x_out = -0.3498 * int(float(x)) / 1932 * 2016 + 524.2 #+0.01*int(y) y_out = -0.355 * int( float(y)) / 1449 * 1504 + 341 - 0.01 * (1400 - int(float(x))) swift.set_position(x=x_out, y=y_out, z=15) swift.set_wrist(90) time.sleep(0.2) #down = input("Down? ") swift.set_pump(True) swift.set_position(x=x_out, y=y_out, z=0) #up = input("Up? ") time.sleep(0.2) swift.set_position(x=x_out, y=y_out, z=30) ang1 = swift.get_servo_angle()[0] x = data[3] y = data[2] r = data[4] x_out = -0.3498 * int(float(x)) + 524.2 #+0.01*int(y) y_out = -0.355 * int(float(y)) + 341 - 0.01 * (1400 - int(float(x))) swift.set_position(x=x_out, y=y_out, z=15) time.sleep(6) ang2 = swift.get_servo_angle()[0] swift.set_wrist(90 + (ang2 - ang1)) #down = input("Down? ") time.sleep(0.5)
class UArmHandler(object): def __init__(self, ui): super(UArmHandler, self).__init__() self.ui = ui self.arm = None self.port = None self.cmd_que = queue.Queue(100) self.cmd_thread = XArmThread(self.cmd_que, check=False) self.cmd_thread.signal.connect(self.run_cmd) self.cmd_thread.start() self.report_que = queue.Queue() self.report_thread = XArmThread(self.report_que, check=True) self.report_thread.signal.connect(self.update_ui) self.report_thread.start() def run_cmd(self, item): try: if self.arm and self.arm.connected: func = getattr(self.arm, item['cmd']) # log(item['cmd']+':', func(*item.get('args', []), **item.get('kwargs', {}))) ret = func(*item.get('args', []), **item.get('kwargs', {})) if item['cmd'] == 'get_position' and isinstance( ret, list) and len(ret) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': ret, 'angles': None } }) elif item['cmd'] == 'get_polar' and isinstance( ret, list) and len(ret) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': [None, None, None, *ret], 'angles': None } }) elif item['cmd'] == 'get_servo_angle' and isinstance( ret, list) and len(ret) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': None, 'angles': ret } }) elif item['cmd'] == 'get_device_info': self.report_que.put({'type': 'info', 'item': ret}) elif item['cmd'] == 'get_mode' or item['cmd'] == 'set_mode': self.report_que.put({ 'type': 'mode', 'item': { 'mode': ret } }) # if isinstance(ret, int) and ret == TCP_OR_JOINT_LIMIT: # self.ui.reset_flag() logger.debug('cmd: {}, ret:{}, args: {}, kwargs: {}'.format( item['cmd'], ret, item.get('args', []), item.get('kwargs', {}))) else: self.cmd_que.queue.clear() self.report_connected_callback({'connected': False}) logger.debug('cmd: {}, ret: xArm is not connected'.format( item['cmd'])) except: pass def connect(self, port): try: logger.debug('try connect to {}'.format(port)) if self.arm and self.arm.connected: logger.info('disconnect from {}'.format(self.port)) self.arm.disconnect() except Exception as e: print(e) threading.Thread(target=self.connnect_thread, args=(port, ), daemon=True).start() def connnect_thread(self, port): try: self.port = port self.arm = SwiftAPI(port=port, do_not_open=True) self.arm.connect() if not self.arm.connected: time.sleep(0.5) self.arm.waiting_ready() self.report_connected_callback({'connected': True}) device_info = self.arm.get_device_info() self.report_que.put({'type': 'info', 'item': device_info}) mode = self.arm.get_mode() self.report_que.put({'type': 'mode', 'item': {'mode': mode}}) position = self.arm.get_position() if isinstance(position, list) and len(position) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': position, 'angles': None } }) polar = self.arm.get_polar() if isinstance(polar, list) and len(polar) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': [None, None, None, *polar], 'angles': None } }) angles = self.arm.get_servo_angle() if isinstance(angles, list) and len(angles) >= 3: self.report_que.put({ 'type': 'location', 'item': { 'position': None, 'angles': angles } }) return True except Exception as e: # print(e) self.report_connected_callback({'connected': False}) def disconnect(self): try: if self.arm and self.arm.connected: self.arm.disconnect() self.report_connected_callback({'connected': False}) # logger.info('diconnect from {}'.format(self.addr)) except Exception as e: print(e) def update_ui(self, data): item = data['item'] if data['type'] == 'timeout': if not self.arm or not self.arm.connected: self.ui.update_connect_status(False) elif data['type'] == 'connect': self.ui.update_connect_status(item['connected']) elif data['type'] == 'location': pos = item['position'] angles = item['angles'] if angles: self.ui.axis_ui.update_joints(angles) if pos: self.ui.cartesian_ui.update_cartesians(pos) elif data['type'] == 'info': self.ui.update_device_info(item) elif data['type'] == 'mode': self.ui.update_mode(item['mode']) def report_connected_callback(self, item): self.report_que.put({'type': 'connect', 'item': item}) def put_cmd_que(self, item): self.cmd_que.put(item)
ret = swift.get_power_status() print('power status: {}'.format(ret)) ret = swift.get_device_info() print('device info: {}'.format(ret)) ret = swift.get_limit_switch() print('limit switch: {}'.format(ret)) ret = swift.get_gripper_catch() print('gripper catch: {}'.format(ret)) ret = swift.get_mode() print('mode: {}'.format(ret)) print('set mode:', swift.set_mode(1)) ret = swift.get_mode() print('mode: {}'.format(ret)) ret = swift.get_servo_attach(servo_id=2) print('servo attach: {}'.format(ret)) ret = swift.get_servo_angle() print('servo angle: {}'.format(ret)) def print_callback(ret, key=None): print('{}: {}'.format(key, ret)) swift.get_polar(wait=False, callback=functools.partial(print_callback, key='polar')) swift.get_position(wait=False, callback=functools.partial(print_callback, key='position')) swift.flush_cmd() swift.disconnect()