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)
