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)
class uarm: def __init__(self): logger.setLevel(logger.VERBOSE) self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, callback_thread_pool_size=1) self.swift.waiting_ready() self.swift.set_speed_factor(factor=20) self.swift.set_position(x=239.43, y=0, z=170) def jump(self, press_time): self.swift.set_position(x=239.43, y=0, z=-30) print("robot:%f" % press_time) time.sleep(press_time) self.swift.set_position(x=239.43, y=0, z=170) # if __name__ == '__main__': # arm = uarm() # arm.jump(0.75) # time.sleep(5) # arm.jump(0.75)
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!")
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()
device_info = swift1.get_device_info() print(swift1.port, device_info) firmware_version = device_info['firmware_version'] if firmware_version and not firmware_version.startswith( ('0.', '1.', '2.', '3.')): swift1.set_speed_factor(0.00001) swift2.waiting_ready() device_info = swift2.get_device_info() print(swift2.port, device_info) firmware_version = device_info['firmware_version'] if firmware_version and not firmware_version.startswith( ('0.', '1.', '2.', '3.')): swift2.set_speed_factor(0.00001) X = 0 Y = 1 Z = 2 R = 3 swift1.set_servo_detach() swift1.register_report_position_callback(lambda position: swift2.set_position( x=position[X], y=position[Y], z=position[Z], speed=100000000, timeout=1)) # swift1.register_report_position_callback(lambda position: swift2.set_position(x=position[X] + random.random() * 5, y=position[Y] + random.random() * 5 , z=position[Z], speed=100000000, timeout=1)) swift1.set_report_position(interval=0.001) input() swift1.disconnect()
swift.waiting_ready() # wait the rebot ready print(swift.get_device_info()) move_speed = 100 time.sleep(5) # <! must wait the effector check itself rtn = swift.get_mode(wait=True, timeout=10) # <! make sure the work mode is 5 print(rtn) if rtn != 5: swift.set_mode(5) time.sleep(5) # <! must wait the effector check itself swift.set_position(x=150, y=150, z=150, speed=move_speed, wait=True, timeout=10, cmd='G0') # print( swift.get_position() ) # print( swift.get_servo_angle() ) # print( swift.send_cmd_sync('P2243') ) while True: swift.set_position(x=400, y=0, z=50, speed=move_speed, wait=True, timeout=10, cmd='G0')
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 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.set_position(x=250, y=210, z=120, speed=50000, wait=True)
class uarmRobotClass(object): def __init__(self): self.x_pos = 0 self.y_pos = 0 self.tcp_rot = 0 self.z_pos = 0 self.pick = 0 self.connect = 0 self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.swift.waiting_ready(timeout=3) self.swift.set_speed_factor(0.5) self.swift.waiting_ready(timeout=3) self.swift.disconnect() def openPort(self): self.swift.connect() #swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.swift.waiting_ready(timeout=3) self.swift.set_speed_factor(0.5) self.swift.waiting_ready(timeout=3) self.swift.set_position(150, 0, 50, wait=True) def closePort(self): self.swift.disconnect() self.swift.waiting_ready(timeout=3) def move(self): Index = 0 pick = 0 connect = 0 while True: if (dummy == False) and (connect == 0) and (self.connect == 1): self.openPort() ret = self.swift.get_position(wait=True) print("ret values %d, %d, %d" % (ret[0], ret[1], ret[2])) x_pos = ret[0] y_pos = ret[1] tcp_rot = ret[2] print("connected") connect = 1 if (dummy == False) and (connect == 1) and (self.connect == 0): self.closePort() connect = 0 print("disconnnected") if (self.x_pos != 0) or (self.y_pos != 0) or (self.tcp_rot != 0) or (self.z_pos != 0) or (self.pick != 0) or (self.connect != 0): if logLevel > 1: print("index = %d: x_pos = %d; y_pos = %d; z_pos = %d; tcp_rot = %d; pick = %d; connect = %d" % (Index, self.x_pos, self.y_pos, self.z_pos, self.tcp_rot, self.pick, self.connect)) Index = Index + 1 if (dummy == False) and (connect == 1) and ((self.x_pos != 0) or (self.y_pos != 0) or (self.tcp_rot != 0) or (self.z_pos != 0)): self.swift.set_position(x=self.x_pos, y=self.y_pos, z=self.z_pos, wait=True, relative=True); ret = self.swift.get_position(wait=True) if logLevel > 0: print("ret values %d, %d, %d" % (ret[0], ret[1], ret[2])) if (dummy == False) and (pick == 0) and (self.pick != 0): self.swift.set_pump(on=True) pick = 1 if (dummy == False) and (pick == 1) and (self.pick == 0): self.swift.set_pump(on=False) pick = 0 time.sleep(0.001);
#ret = swift.get_device_info() #print('device info: {}'.format(ret)) #ret = swift.get_limit_switch() #print('limit switch: {}'.format(ret)) #test=swift.set_acceleration(0.0001) #swift.set_speed_factor(3) #swift.set_position(114, -100, 50, wait=True) #n=0 #while n <=200 : # swift.set_position(0, 1, 0, relative=True, wait=True, ) # swift.get_position(wait=False, callback=functools.partial(print_callback, key='position')) # n = n + 1 swift.waiting_ready(timeout=10) swift.set_position(140, -80, 50, wait=True) swift.set_position(114, 122, 50, wait=True) swift.set_position(114, 122, 0, wait=True) swift.set_position(114, 122, 50, wait=True) swift.set_position(300, 120, 50, wait=True) swift.set_position(114, 122, 50, wait=True) swift.set_position(114, 122, 0, wait=True) swift.set_position(114, 122, 50, wait=True) swift.set_position(110, 120, 50, wait=True) swift.set_position(114, 122, 50, wait=True) swift.set_position(114, 122, 0, wait=True) swift.set_position(114, 122, 50, wait=True) swift.set_position(220, 0, 50, wait=True) swift.set_position(114, 122, 50, wait=True) swift.set_position(114, 122, 0, wait=True) swift.set_position(114, 122, 50, wait=True)
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) while True: swift.set_position(x=267,y=-0,z=30,speed=200,cmd='G0') time.sleep(0.5) swift.set_position(x=267,y=-0,z=9,speed=200,cmd='G0') time.sleep(3) swift.set_position(x=200,y=-23.28,z=9,speed=200,cmd='G1') time.sleep(0.5) swift.set_position(x=200,y=-23.28,z=30,speed=200,cmd='G0') time.sleep(0.5) swift.set_position(x=212,y=-114,z=30,speed=200,cmd='G0') time.sleep(0.5) swift.set_position(x=212,y=-114,z=8,speed=200,cmd='G0') time.sleep(0.5) swift.set_position(x=212,y=-114,z=30,speed=200,cmd='G0') time.sleep(1)
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) speed = 10 swift.reset(wait=True, speed=speed) swift.set_digital_output(pin=45, value=1) # yellow led swift.flush_cmd() print('Begin') swift.set_position(x=200, y=-82, z=100, speed=speed, wait=True) swift.set_position(x=150, y=47, z=30, speed=speed, wait=True) swift.set_position(x=150, y=71, z=30, speed=speed, wait=True) swift.set_position(x=300, y=-150, z=150, speed=speed, wait=True) swift.set_position(x=150, y=-150, z=30, speed=speed, wait=True) swift.set_position(x=300, y=-150, z=150, speed=speed, wait=True) swift.set_position(x=300, y=-45, z=150, speed=speed, wait=True) swift.set_position(x=150, y=-73, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-150, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-73, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-85, z=30, speed=speed, wait=True) swift.set_position(x=150, y=134, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-73, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-30, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-28, z=30, speed=speed, wait=True) swift.set_position(x=150, y=150, z=30, speed=speed, wait=True)
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) speed = 100 swift.reset(wait=True, speed=speed) swift.set_digital_output(pin=49, value=1) # green led swift.flush_cmd() print('Begin') swift.set_position(x=200, y=-19, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-17, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-12, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-11, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-9, z=100, speed=speed, wait=True) swift.set_position(x=150, y=-67, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-109, z=30, speed=speed, wait=True) swift.set_position(x=150, y=56, z=30, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=150, y=-27, z=30, speed=speed, wait=True) swift.set_position(x=300, y=-101, z=150, speed=speed, wait=True) swift.set_position(x=150, y=-150, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-150, z=30, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=150, y=150, z=30, speed=speed, wait=True)
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.00005) swift.set_mode(0) speed = 100000 # swift.reset(speed=speed) while swift.connected: swift.set_position(x=300, y=0, z=150, speed=speed) swift.set_position(z=50) swift.set_position(z=150) swift.set_position(x=200, y=100, z=100) swift.set_position(z=50) swift.set_position(z=150) swift.set_position(x=200, y=0, z=150) swift.set_position(z=50) swift.set_position(z=150) swift.set_polar(stretch=200, rotation=90, height=150, speed=speed) swift.set_polar(stretch=200, rotation=45, height=150, speed=speed) swift.set_polar(stretch=200, rotation=135, height=150, speed=speed) swift.set_polar(stretch=200, rotation=135, height=90, speed=speed) swift.set_polar(stretch=200, rotation=135, height=200, speed=speed)
from uarm.wrapper import SwiftAPI """ api test: move """ 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'] swift.set_mode(0) swift.reset(wait=True, speed=250) swift.set_position(x=300, speed=250) swift.set_position(y=200) swift.set_position(z=100) swift.flush_cmd(wait_stop=True) swift.set_polar(stretch=300, rotation=0, height=100, wait=True) swift.set_polar(stretch=300, speed=250) swift.set_polar(rotation=0) swift.set_polar(height=100) print() swift.flush_cmd() time.sleep(3) swift.disconnect()
class uArm(): def __init__(self): self.scope = 10 self.x0 = 160 self.y0 = 0 self.swift = SwiftAPI(filters={'hwid':'USB VID:PID=2341:0042'}) self.swift.waiting_ready(timeout=3) # self.swift.set_speed_factor(0.005) # if you change this, be prepared for different movements! self.swift.set_mode(mode=0) time.sleep(0.5) self.swift.set_servo_angle(angle=90) self.swift.set_wrist(angle=90) self.swift.set_position(x=200,y=0,z=20) # start it off with a salute self.swift.set_buzzer(frequency=1000, duration=1) # signal ready self.lstValidCharSet = ['?','A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P','Q','R','S','T','U','V','W','X','Y','Z',\ '-','0','1','2','3','4','5','6','7','8','9'] self.lstLetter = [self.QuestionMark, self.LetterA, self.LetterB, self.LetterC, self.LetterD, self.LetterE, self.LetterF,\ self.LetterG, self.LetterH, self.LetterI, self.LetterJ, self.LetterK, self.LetterL, self.LetterM, self.LetterN,\ self.LetterO, self.LetterP, self.LetterQ, self.LetterR, self.LetterS, self.LetterT, self.LetterU, self.LetterV,\ self.LetterW, self.LetterX, self.LetterY, self.LetterZ, self.Hyphen, self.Number0, self.Number1, self.Number2,\ self.Number3, self.Number4, self.Number5, self.Number6, self.Number7, self.Number8, self.Number9] def __del__(self): input("PLEASE SUPPORT uARM ARM!!, then strike ENTER to continue ...") self.swift.set_buzzer(frequency=600, duration=2) self.swift.set_position(x=200,y=0,z=20) self.swift.flush_cmd() self.swift.disconnect() del self.swift self.swift = None def arm(self): """ Using this method to allow raw access to the uArm if required """ return self.swift def insert_pen(self): self.swift.set_buzzer(frequency=1000, duration=0.5) # signal ready self.swift.set_servo_angle(angle=90) time.sleep(0.5) self.swift.set_wrist(angle=90) time.sleep(0.5) self.swift.set_position(x=200,y=0,z=0) while (self.swift.get_is_moving()): continue input("Set pen in universal holder, then strike ENTER to continue ...") self.swift.set_position(x=200,y=0,z=10) return def pen_up(self): while (self.swift.get_is_moving()): continue x, y, z = self.swift.get_position() self.swift.set_position(x, y, 10) time.sleep(0.5) return 10 def pen_down(self): while (self.swift.get_is_moving()): continue x, y, z = self.swift.get_position() self.swift.set_position(x, y, 0) time.sleep(0.5) return 0 def setScope(self, strName): """ based upon the length of strName, determine the scope (char width) and starting X, Y positions assuming that the center of the page is 160,0 x extent is 110 - 210, y extent 80 - (-80) (x axis is PARALLEL to the arm, short edge of the paper) """ if type(strName) == str: strName = strName[:26] # going to truncate user input to a 26 characters max intLenName = len(strName) if (intLenName < 4): self.scope = 40.0 # keeping it real else: self.scope = math.floor(160.0/(intLenName * 1.1)) self.x0 = 160 - (0.5 * self.scope) self.y0 = self.scope * intLenName * 1.1 / 2 return def LetterSelect(self, c): """ given char c, return the plotting function index 0 resolves to the question mark character """ index = 0 if type(c) == str: if c == ' ': return self.SpaceBar else: c = c.upper() if c in self.lstValidCharSet: index = self.lstValidCharSet.index(c) - self.lstValidCharSet.index('A') + 1 # 0th item is '?' # if c in ['A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P','Q','R','S','T','U','V','W','X','Y','Z']: # index = ord(c) - ord('A') + 1 # using question mark as the 0th index item return self.lstLetter[index] # return the function to use
input("\n* アーム,ロボットから手を放してください,安全が確認できたらEnterキーを押してください") swift.reset(speed=100000, wait=True) print("\n * セル移動チェック * \n 入力したポジションに移動することができます \n 数値以外を入力すると終了します \n 特に X 0, Y 3 や X 0, Y 4 に移動することが出来るか必ず確認してください") while(True): try: selectX = int(input("X (0 ~ 7) >> ")) selectY = int(input("Y (0 ~ 7) >> ")) targetX = target[selectX][selectY][X] # sector[0][X] + ((len[X] / 7) * selectX) targetY = target[selectX][selectY][Y] #sector[0][Y] + ((len[Y] / 7) * selectY) swift.set_position(targetX, targetY, 30.0, wait=True) except: break if(input("セットアップをやりなおす場合は n を入力しEnterを押してください \n n 以外が入力された場合セットアップを完了します") != 'n'): setupDisable = False def moveArmByReversiPredicted(predict): predictPosition = predict swift.reset(speed=100000, wait=True) swift.set_pump(on=True, wait=True) time.sleep(3) swift.flush_cmd()
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) speed = 2 swift.reset(wait=True, speed=speed) swift.set_digital_output(pin=50, value=1) # set led to red swift.flush_cmd() print('Begin') swift.set_position(x=200, y=-1, z=100, speed=speed, wait=True) swift.set_position(x=200, y=0, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-2, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-5, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-2, z=100, speed=speed, wait=True) swift.set_position(x=200, y=2, z=100, speed=speed, wait=True) swift.set_position(x=200, y=3, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-5, z=100, speed=speed, wait=True) swift.set_position(x=200, y=2, z=100, speed=speed, wait=True) swift.set_position(x=200, y=4, z=100, speed=speed, wait=True) swift.set_position(x=200, y=0, z=100, speed=speed, wait=True) swift.set_position(x=200, y=0, z=100, speed=speed, wait=True) swift.set_position(x=200, y=1, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-6, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-1, z=100, speed=speed, wait=True) swift.set_position(x=200, y=0, z=100, speed=speed, wait=True)
# Copyright (c) 2018, UFactory, Inc. # All rights reserved. # # Author: Vinman <*****@*****.**> <*****@*****.**> import os import sys import time import threading sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from uarm.wrapper import SwiftAPI from uarm.tools.list_ports import get_ports 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) while True: swift.set_position(x=170,y=0,z=12,speed=200,cmd='G0') swift.set_position(x=170,y=0,z=18,speed=200,cmd='G0') swift.set_position(x=170,y=0,z=12,speed=200,cmd='G0') swift.set_position(x=170,y=0,z=18,speed=200,cmd='G0') time.sleep(2)
# Copyright (c) 2018, UFactory, Inc. # All rights reserved. # # Author: Vinman <*****@*****.**> <*****@*****.**> import os import sys import time import threading sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from uarm.wrapper import SwiftAPI from uarm.tools.list_ports import get_ports 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) while True: swift.set_position(x=151, y=0, z=20, speed=200, cmd='G0') time.sleep(1) swift.set_position(x=151, y=0, z=8, speed=200, cmd='G0') swift.set_position(x=330, y=0, z=30, speed=200, cmd='G0') time.sleep(1)
from uarm.wrapper import SwiftAPI from uarm.tools.list_ports import get_ports 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) cmd_s = 'G1' speed_s = 10 delay = 3 swift.set_position(x=260,y=-50,z=30,speed=speed_s,cmd=cmd_s) time.sleep(delay) swift.set_position(x=260,y=-50,z=14,speed=speed_s,cmd=cmd_s) time.sleep(delay) swift.set_position(x=260,y=50,z=14,speed=speed_s,cmd=cmd_s) time.sleep(delay) swift.set_position(x=180,y=50,z=14,speed=speed_s,cmd=cmd_s) time.sleep(delay) swift.set_position(x=180,y=-50,z=14,speed=speed_s,cmd=cmd_s) time.sleep(delay) swift.set_position(x=260,y=-50,z=14,speed=speed_s,cmd=cmd_s) time.sleep(delay) swift.set_position(x=260,y=-50,z=30,speed=speed_s,cmd=cmd_s)
class uart: available_pixel = {} #rgb values of all the paints swift = None #robot arm object device_info = None firmware_version = None image = None #image you're trying to paint canvas = None #image of the canvas as you're working on it canvas_corners = None #points of the four corners of the canvas (in robot arm coords) ptransform = None #contains the warped image of M = None #transformation matrix xScale = None yScale = None # # __init__ # im = the image you're trying to paint # pixels = the dictionary of colors you have access to # initialized = a list of booleans determining which values you will initialize # [ True = available_pixel uses pixels parameter otherwise use defaults, # True = set swift to SwiftAPI object otherwise set them to None, # True = set image to a blank white 200x200 image, # True = calibrate canvas_corners using setFourCorners otherwise set to a preset # True = set ptransform using the webcam # ] # def __init__(self, im, pixels, initialized): if initialized[0]: self.available_pixel = pixels else: self.available_pixel = {'red':[255,0,0], 'green':[0,255,0], 'blue':[0,0,255],'magenta':[255,0,255], 'tomato':[255,99,71], 'lawn green':[124,252,0]} if initialized[1]: self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.device_info = self.swift.get_device_info() self.firmware_version = self.device_info['firmware_version'] self.swift.set_mode(0) if initialized[2]: self.image = im if initialized[3] and initialized[1]: print("moving") self.swift.set_position(x=150, y=0, z=50, speed = 10000, cmd = "G0") # self.swift.set_wrist(20) # time.sleep(1) # self.swift.set_wrist(90) print("Setting four corners; input tl, tr, bl or br") self.canvas_corners = self.setFourCorners() else: self.swift.set_position(x=150, y=0, z=50, speed = 10000, cmd = "G0") self.canvas_corners = [ [263,50,103], #tl [263,-50,103],#tr [241,50,-12],#bl [241,-50,-12]]#br print("Setting four corners to default coordinates") if initialized[4]: _, cap = cv2.VideoCapture(0).read() self.ptransform = perspective.PerspectiveTransform(cap) self.M = self.get_m(200,200) self.xScale = self.get_scale(len(im[0]),[self.canvas_corners[0],self.canvas_corners[1]]) self.yScale = self.get_scale(len(im),[self.canvas_corners[0],self.canvas_corners[2]]) print("Arm all set up!") # # new xy to xyz function using algebra/geometry # def xy_to_xyz2(self, xy): #print("xy", xy) #print("xscale", self.xScale) #print("yscale", self.yScale) out = np.add(np.multiply(xy[0],self.xScale) + np.multiply(xy[1],self.yScale), self.canvas_corners[0]) print(out) return out # # GET SCALE # def get_scale(self, pix, corners): dif = np.subtract(corners[0], corners[1]) return -(dif/pix) # # HEAT MAP # def generate_heatmap(self): image = self.image.astype(dtype='int32') canvas = self.ptransform.warped.astype(dtype='int32') subtraction = np.subtract(image,canvas) print(subtraction) heatmap = np.full(im.shape,255, dtype='uint8') print(heatmap.shape) for i in range(subtraction.shape[0]): for j in range(subtraction.shape[1]): if (subtraction[i][j] < 0): heatmap[i][j][0] -= abs(subtraction[i][j]) heatmap[i][j][1] -= abs(subtraction[i][j]) elif (subtraction[i][j] > 0): heatmap[i][j][2] -= abs(subtraction[i][j]) heatmap[i][j][1] -= abs(subtraction[i][j]) return heatmap # # GETS CLOSEST COLOR # def get_closest_color(self, chosen_pixel): available_pixel = self.available_pixel distances = [] for key, value in available_pixel.items(): a1 = np.asarray(value) c1 = np.asarray(chosen_pixel) curr_dist = np.linalg.norm(a1 - c1) distances += [curr_dist] if(curr_dist == min(distances)): curr_key = key return curr_key # # move_to_file # def move_to_file(self, filename): var = [] count = 0 lines = open(filename, "r").read().split('\n') x,y,z,f,angle = 0 moveArm,moveWrist = False for i in range(len(lines)): for word in lines[i].split(' '): if(word is 'G0'): moveArm = True if(word[0] is 'X'): x = float(word[1:]) elif(word[0] is 'Y'): y = float(word[1:]) elif(word[0] is 'Z'): z = float(word[1:]) elif(word[0] is 'F'): f = float(word[1:]) elif(word is 'WA'): moveWrist = True angle = float(word[1:]) if(moveArm): self.swift.set_position(x=x, y=y, z=z, speed =f, cmd = "G0") moveArm = False time.sleep(1) if(moveWrist): self.swift.set_wrist(angle) moveWrist = False time.sleep(1) coordinates.close() # # SETTING FOUR CORNERS # def setFourCorners(self): speed_s = 10000 delay = 1 cmd_s = 'G0' todo = 4 coords = [[], [], [], []] while todo >0: key = input() if key == "tr": newCoord = self.swift.get_position() coords[1] = newCoord todo -= 1 print("Top right coordinate saved as ", newCoord) elif key == "tl": newCoord = self.swift.get_position() coords[0] = newCoord todo -= 1 print("Top left coordinate saved as", newCoord) elif key == "bl": newCoord = self.swift.get_position() coords[2] = newCoord todo -= 1 print("Bottom left coordinate saved as", newCoord) elif key == "br": newCoord = self.swift.get_position() coords[3] = newCoord todo -= 1 print("Bottom right coodirnate saved as", newCoord) return coords # # SAVED COORDS TO FILE # def saveCoordsToFile(self, fn): delay = 1 coords = [] while True: key = input() if key == "save": newCoord = swift.get_position() coords.append(newCoord) print("New coordinate saved as" + str(newCoord)) elif key == "done": break elif key.isdigit(): coords.append(int(key)) if os.path.exists(fn + ".uar"): os.remove(fn + ".uar") file = open(fn + ".uar", "w+") for c in coords: if not check(c): file.write("G0 X%f Y%f Z%f F5000\n" %(c[0], c[1], c[2])) else: self.set_wrist(c) file.write("WA " %(c)) coordinates.close() moveTo(fn + ".uar") return coords def check(inp): try: num_float = float(inp) return True except: return False # # GET M # def get_m(self, width, height): A = np.transpose(self.canvas_corners) print(A) B = [[0,0,1],[width,0,1],[0,height,1],[width,height,1]] B = np.transpose(B) print(B) pinvB = np.linalg.pinv(B) print(pinvB) M = np.matmul(A, np.linalg.pinv(B)) print(M) return M # # xytoxyz # def xy_to_xyz(self,xy): xyz = [xy[0],xy[1],1] xyz = np.transpose(xyz) return np.matmul(self.M,xyz) # # go to position # def go_to_position(self, xyz, f): print('going to : ', xyz) self.swift.set_position(x=xyz[0], y=xyz[1], z=xyz[2], speed = f, cmd = "G0") #: time.sleep(1) # # draw a line # # start and end: [x,y] def draw_line(self, start, end): startxyz = self.xy_to_xyz2(start) endxyz = self.xy_to_xyz2(end) start_pre = [startxyz[0]-20, startxyz[1], startxyz[2]] end_post = [endxyz[0]-20, endxyz[1], endxyz[2]] print("going to start pre") self.go_to_position(start_pre, 10000) print("going to start") self.go_to_position(startxyz, 5000) print("going to end") self.go_to_position(endxyz, 5000) print("going to end post") self.go_to_position(end_post, 10000) # # # draws a line, by moving across a list of points # def draw_line2(self, points): startxyz = self.xy_to_xyz2(points[0]) endxyz = self.xy_to_xyz2(points[-1]) start_pre = [startxyz[0]-5, startxyz[1], startxyz[2]] end_post = [endxyz[0]-5, endxyz[1], endxyz[2]] #print("going to start pre") self.go_to_position(start_pre, 10000) for point in points: point_xyz = self.xy_to_xyz2(point) self.go_to_position(point_xyz, 5000) #print("going to end post") self.go_to_position(end_post, 10000) # # # draws a line, by moving across a list of points # does NOT go to pre and post painting position # def draw_line3(self, points): startxyz = self.xy_to_xyz2(points[0]) endxyz = self.xy_to_xyz2(points[-1]) #print("going to start pre") #self.go_to_position(start_pre, 10000) for point in points: point_xyz = self.xy_to_xyz2(point) self.go_to_position(point_xyz, 5000)
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) print(swift.set_polar(stretch=150, rotation=90, height=150, wait=True)) swift.set_polar(stretch=300, rotation=90, height=30, speed=100000, wait=True) print(swift.set_polar(stretch=300, rotation=90, height=30, wait=True)) ''' speed = 30 swift.reset(wait=True, speed=speed) swift.set_position(x=150, y=150, z=30, speed=speed, wait=True) print('Position 1 reached') swift.set_position(x=200, y=-150, z=30, speed=speed, wait=True) print('Position 2 reached') swift.set_position(x=250, y=0, z=150, speed=speed, wait=True) print('Position 3 reached') swift.set_position(x=150, y=100, z=30, speed=speed, wait=True) print('Position 4 reached') print('begin') swift.set_position(x=200, y=-21, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-7, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-54, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-72, z=100, speed=speed, wait=True) swift.set_position(x=200, y=150, z=100, speed=speed, wait=True) swift.set_position(x=150, y=24, z=30, speed=speed, wait=True)
# swift.set_speed_factor(0.0005) #set for general mode swift.set_mode(0) #LED pin numbers red = 50 yellow = 45 green = 49 speed = 10 swift.reset(wait=True, speed=speed) #swift.set_digital_output(pin=TODO, value=1) # set led based on program type swift.flush_cmd() print('Begin') swift.set_position(x=200, y=23, z=100, speed=speed, wait=False) swift.set_position(x=200, y=136, z=100, speed=speed, wait=False) swift.set_position(x=200, y=142, z=100, speed=speed, wait=False) swift.set_position(x=200, y=150, z=100, speed=speed, wait=False) swift.set_position(x=150, y=127, z=30, speed=speed, wait=False) swift.set_position(x=300, y=145, z=150, speed=speed, wait=False) print('Ending') swift.reset(wait=True, speed=speed) #swift.set_digital_output(pin=TODO, value=0) # set led based on program type swift.flush_cmd() time.sleep(5) swift.disconnect() print('Finished')
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) cmd_s = 'G1' while True: # enter photo app swift.set_position(x=267, y=-38, z=30, speed=200, cmd=cmd_s) time.sleep(0.4) swift.set_position(x=267, y=-38, z=9.8, speed=200, cmd=cmd_s) time.sleep(0.3) swift.set_position(x=267, y=-38, z=30, speed=200, cmd=cmd_s) time.sleep(0.5) #take a photo swift.set_position(x=211, y=-93, z=30, speed=200, cmd=cmd_s) time.sleep(0.4) swift.set_position(x=211, y=-93, z=10, speed=200, cmd=cmd_s) time.sleep(0.3) swift.set_position(x=211, y=-93, z=30, speed=200, cmd=cmd_s) time.sleep(1) # exit photo app
from uarm.wrapper import SwiftAPI swift = SwiftAPI() swift.waiting_ready(timeout=10) swift.set_position(100, -120, 50, wait=True) test = swift.get_position() print(test) swift.set_position(300, -120, 50, wait=True) test = swift.get_position() print(test) swift.set_position(300, 120, 100, wait=True) test = swift.get_position() print(test) swift.set_position(100, 120, 100, wait=True) test = swift.get_position() print(test) swift.set_position(120, 0, 50, wait=True) test = input("the pump will be set on, press enter to continue") swift.set_pump(on=True) swift.set_position(200, 0, 50, wait=True) test = input("the pump will be set off, press enter to continue") swift.set_pump(on=False) swift.set_position(120, 0, 50, wait=True)
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 swift.set_position(x=pos[X], y=pos[Y] + (-d[seq] * bias), z=pos[Z] - 15, speed=10000000) # return swift.set_position(x=pos[X], y=pos[Y] + (-d[seq] * bias), z=pos[Z] + 5, speed=10000000) swift.flush_cmd() # while True: # for seq in seqs: # # move
from uarm.wrapper import SwiftAPI swift = SwiftAPI() swift.waiting_ready(timeout=10) swift.set_position(140, 0, 10, wait=True) swift.set_position(320, 0, 10, wait=True) swift.set_position(140, 0, 10, wait=True) swift.set_speed_factor(4) input( "now the speed of the arm has been multiply by 4, it will move faster. press enter to continue" ) swift.set_position(140, 0, 10, wait=True) swift.set_position(320, 0, 10, wait=True) swift.set_position(140, 0, 10, wait=True) swift.set_speed_factor(1)
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) # swift.flush_cmd(wait_stop=True)
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) speed = 10000 swift.reset(wait=True, speed=speed) swift.set_digital_output(pin=49, value=1) # green led swift.flush_cmd() print('Begin') swift.set_position(x=200, y=49, z=100, speed=speed, wait=True) swift.set_position(x=200, y=-109, z=100, speed=speed, wait=True) swift.set_position(x=300, y=-132, z=150, speed=speed, wait=True) swift.set_position(x=300, y=17, z=150, speed=speed, wait=True) swift.set_position(x=150, y=-117, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-90, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-95, z=30, speed=speed, wait=True) swift.set_position(x=150, y=-150, z=30, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=300, y=141, z=150, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=300, y=150, z=150, speed=speed, wait=True) swift.set_position(x=300, y=-150, z=150, speed=speed, wait=True) swift.set_position(x=150, y=-79, z=30, speed=speed, wait=True)