import sys
import time
import functools
import threading
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))
from uarm.wrapper import SwiftAPI
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.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,
#
# 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)
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)
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)
def initialize():
global cartesian
test_a = True
test_b = True
data_start() # saving data
#convert cartesian coordinates to polar coordinates
if(cartesian):
print(Fore.BLUE + "Detecting cartesian coordinates, changing to polar")
#test to check converting cartesian to polar
if(test_converting(cartesian)):
print(Fore.GREEN + "TEST OK")
print(Style.RESET_ALL)
else:
print(Fore.RED + "TEST FAILED")
print(Style.RESET_ALL)
#debug outputs
# print(default_stretch)
# print(default_rotation)
# print(default_height)
change_variables_to_polar()
print(Fore.GREEN + "Conversion from cartesian to polar successfull")
print(Style.RESET_ALL)
time.sleep(2)
else:
print(Fore.BLUE + "Default polar coordinates are set")
print(Style.RESET_ALL)
try:
global serial_name
global ser
ser = serial.Serial('/dev/ttyUSB0',115200,timeout=2) #arduino serial
arduino_reset()
print(Fore.BLUE + "Arduino UNO\n")
except:
test_a = False
print(Fore.RED + "FAILED CONNECT TO ARDUINO!")
print(Style.RESET_ALL)
#setting up uArm SwiftPro
try:
global swift
#need to change
sys.path.append(os.path.join(os.path.dirname(__file__), '../uArm-Python-SDK')) #link to uArm SwiftPro python3.x.x library
swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
swift.waiting_ready(timeout=5)
swift.set_mode(0)
device_info = swift.get_device_info()
default_robot_position()
print(Fore.BLUE)
print(device_info)
print("\n")
except:
test_b = False
print(Fore.RED + "FAILED CONNECT TO uArm SwiftPro")
print(Style.RESET_ALL)
if test_a and test_b:
print(Fore.GREEN + "DONE!\n")
print(Style.RESET_ALL)
return
import os
import sys
import time
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))
from uarm.wrapper import SwiftAPI
"""
multi sync move
"""
swift1 = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
swift2 = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
# swift3 = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
swift1.waiting_ready()
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)
swift_list = [swift1, swift2]
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)
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()
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)
class MinionArm:
def __init__(self):
self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
self.swift.waiting_ready()
device_info = self.swift.get_device_info()
print(device_info)
firmware_version = device_info['firmware_version']
if firmware_version and not firmware_version.startswith(
('0.', '1.', '2.', '3.')):
self.swift.set_speed_factor(0.0005)
self.speed = 110000
self.test()
self.board = "right"
def test(self):
# self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
self.swift.reset(wait=True, speed=self.speed)
self.move(1250, 0, 150, self.speed)
print(self.swift.get_position())
# self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
def home(self):
print("going home")
self.swift.set_position(x=150, y=0, speed=self.speed * 0.5, wait=True)
self.move(145, 0, 100, self.speed * 1.5)
time.sleep(2)
# self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
def rest(self):
print("going to rest position")
self.swift.set_position(x=150, speed=self.speed * 0.5, wait=True)
self.swift.set_position(x=125, y=0, speed=self.speed * 0.5, wait=True)
self.move(125, 0, 40, self.speed * 0.5)
time.sleep(2)
self.swift.set_buzzer(frequency=1000, duration=0.5)
def move(self, x, y, z, speed):
self.swift.set_position(x, y, z, speed, wait=True)
def flip(self):
print("flipping")
# self.home()
self.swift.set_buzzer(frequency=1000, duration=0.5, wait=True)
if self.board == "right":
self.flip_left()
self.board = "left"
else:
self.flip_right()
self.board = "right"
self.home()
self.swift.set_buzzer(frequency=500, duration=1.0, wait=True)
def flip_left(self):
self.move(180, 0, 150, self.speed)
self.move(240, 0, 150, self.speed)
# self.move(220, 10, 140, self.speed*.2)
self.move(220, 40, 150, self.speed * .2)
# self.move(220, 40, 110, self.speed*.2)
# self.move(220, 60, 100, self.speed*.2)
self.move(220, 50, 30, self.speed * .2)
time.sleep(1)
self.move(180, 50, 20, self.speed * .2)
time.sleep(2)
def flip_right(self):
self.move(180, 0, 150, self.speed)
self.move(240, -10, 150, self.speed)
# self.move(220, -20, 140, self.speed*.2)
self.move(220, -40, 150, self.speed * .2)
# self.move(220, -50, 110, self.speed*.2)
# self.move(220, -60, 100, self.speed*.2)
self.move(220, -60, 20, self.speed * .2)
self.move(180, -50, 20, self.speed * .2)
time.sleep(2)
def move_to_galton_cal_pos(self):
self.move(180, 0, 150, self.speed)
self.move(240, 0, 150, self.speed)
wait = input("waiting")
self.home()
def disconnect(self):
self.swift.disconnect()