def moveRandom():
SERVO_BOTTOM = 0
SERVO_LEFT = 1
SERVO_RIGHT = 2
SERVO_HAND = 3
position_arr=[]
angles_arr=[]
arm = SwiftAPI()
sleep(2.0)
rand = np.random.rand(3)
angles = rand * 180
print("angles are", angles)
arm.flush_cmd()
arm.reset()
arm.set_servo_angle_speed(SERVO_RIGHT, angles[0], wait=True, timeout=100, speed=5000)
arm.set_servo_angle_speed(SERVO_BOTTOM, angles[1], wait=True, timeout=100, speed=5000)
arm.set_servo_angle_speed(SERVO_LEFT, angles[2], wait=True, timeout=100, speed=5000)
a = arm.get_is_moving()
print("the status is", a, "\n")
pos = arm.get_position() # float array of the format [x, y, z] of the robots current location
# pixelpostion= get the pixel position here
print("the position is ", pos, "\n")
position_arr.append(pos)
angles = angles.tolist()
angles_arr.append(angles) # [right bottom left]
sleep(2.0)
def moveRandom():
SERVO_BOTTOM = 0
SERVO_LEFT = 1
SERVO_RIGHT = 2
SERVO_HAND = 3
position_arr = []
pixel_position = []
angles_arr = []
arm = SwiftAPI()
sleep(2.0)
rand = np.random.rand(3)
angles = rand * 180
print("angles are", angles)
arm.flush_cmd()
arm.reset()
arm.set_servo_angle_speed(SERVO_RIGHT,
angles[0],
wait=True,
timeout=100,
speed=5000)
arm.set_servo_angle_speed(SERVO_BOTTOM,
angles[1],
wait=True,
timeout=100,
speed=5000)
arm.set_servo_angle_speed(SERVO_LEFT,
angles[2],
wait=True,
timeout=100,
speed=5000)
a = arm.get_is_moving()
print("the status is", a, "\n")
pos = arm.get_position(
) # float array of the format [x, y, z] of the robots current location
cap = cv2.VideoCapture(0)
cap1 = cv2.VideoCapture(0)
red = detect_w_avg(cap, 600, 166, 84, 80, 286, 255, 255, "red")
print("red", red)
blue = detect_w_avg(cap1, 600, 97, 100, 117, 117, 255, 255, "blue")
print("blue", blue)
pixel_position.append(red)
pixel_position.append(blue)
cv2.destroyAllWindows()
cap.release()
cap1.release()
# pixelpostion= get the pixel position here
print("the position is ", pos, "\n")
position_arr.append(pos)
angles = angles.tolist()
angles_arr.append(angles) # [right bottom left]
sleep(2.0)
return angles, pixel_position
def drop_pipette(swift: SwiftAPI, position: dict):
# Hovering above trash
dict_args = position
dict_args['change_z_by'] = 70
dict_args['wait'] = True
swift.set_position(**dict_args)
# back to normal location (relevant in case of using variables)
dict_args['change_z_by'] = 0
swift.set_position(**dict_args)
current_position = swift.get_position()
robot_move(swift=swift,
position={
'x': current_position[0],
'y': current_position[1],
'z': position['z'],
'e': None,
'speed': None
})
dict_args = position
dict_args['wait'] = True
swift.set_position(**dict_args)
swift.set_wrist(270)
sleep(1)
swift.set_wrist(90)
# Hovering above trash
dict_args = position
dict_args['change_z_by'] = 70
dict_args['wait'] = True
swift.set_position(**dict_args)
dict_args[
'change_z_by'] = 0 # back to normal location(relevant in case of using variables)
# you have to specify all arguments for x, y, z and the speed
swift.set_position(330, 0, 100, speed=1500, timeout=20)
swift.flush_cmd() # avoid follow 5 command timeout
print('\nset X340 ...')
swift.set_position(330, 0, 150, speed=1500)
print('set X320 ...')
swift.set_position(320, 0, 150, speed=1500)
print('set X300 ...')
swift.set_position(300, 0, 150, speed=1500)
print('set X200 ...')
swift.set_position(200, 0, 150, speed=1500)
print('set X190 ...')
swift.set_position(190, 0, 150, speed=1500)
# wait all async cmd return before send sync cmd
swift.flush_cmd()
print('set Z100 ...')
swift.set_position(190, 0, 100, speed=1500, wait=True)
print('set Z150 ...')
swift.set_position(190, 0, 150, speed=1500, wait=True)
swift.set_buzzer()
print('get_position:', swift.get_position())
print('done ...')
while True:
sleep(1)
class RobotMapper:
BOUNDS = (150, -80, 225, 80)
MOVE_SPEED = 1500
LASER_SPEED = 150
# 180 > 150
def __init__(self, dev_port, z_height=180):
self.draw_height = z_height
self.current_map = None
self.running = False
# Set up robot
print("Connecting to robot")
self.swift = SwiftAPI(dev_port=dev_port)
sleep(2)
print("Done Connecting")
self.reset()
def draw_map(self, map):
""" Resize map and prep for use, then start running"""
map = map.fit_to(self.BOUNDS)
self.current_map = map
self.swift.set_position(150,
0,
self.draw_height,
speed=self.MOVE_SPEED,
wait=True)
for cur_pt, next_pt, pen_down in self.current_map:
if not pen_down: continue
self.draw_line(cur_pt, next_pt)
self.reset()
def draw_line(self, from_pt, to_pt):
# Make sure it's a valid request
if from_pt == to_pt: return
LASER_CMD = "G1 X{} Y{} Z{} F{}"
# Get into the correct position before drawing the linn
cur_pos = self.swift.get_position()[:2]
if cur_pos != from_pt:
print("Moving to", from_pt, "from", cur_pos)
self.swift.set_position(x=from_pt[0],
y=from_pt[1],
z=self.draw_height,
speed=self.MOVE_SPEED,
wait=True)
# Actually draw the line
print("Drawing Line: ", from_pt, to_pt)
to_pt = [int(round(to_pt[0])), int(round(to_pt[1]))]
cmd = LASER_CMD.format(to_pt[0], to_pt[1], self.draw_height,
self.LASER_SPEED)
assert self.swift.send_cmd_sync(
cmd) == "ok", "Unable to send robot to using laser command!"
# Set the position normally, to turn off the laser
self.swift.set_position(x=to_pt[0],
y=to_pt[1],
z=self.draw_height,
speed=self.MOVE_SPEED,
wait=True)
def stop(self):
self.running = False
self.reset()
def reset(self):
""" Put the swift in a resting position """
self.swift.set_polar(s=112,
r=90,
h=67,
speed=self.MOVE_SPEED,
wait=True)
speed=5000)
arm.set_servo_angle_speed(SERVO_BOTTOM,
angles[1],
wait=True,
timeout=100,
speed=5000)
arm.set_servo_angle_speed(SERVO_LEFT,
angles[2],
wait=True,
timeout=100,
speed=5000)
a = arm.get_is_moving()
print("the status is", a, "\n")
pos = arm.get_position(
) # float array of the format [x, y, z] of the robots current location
# pixelpostion= get the pixel position here
print("the position is ", pos, "\n")
position_arr.append(pos)
angles = angles.tolist()
angles_arr.append(angles) # [right bottom left]
sleep(2.0)
DAT = np.row_stack((position_arr, angles_arr))
print("try final ", DAT)
np.savetxt("data1.text", DAT, newline=" \n ", fmt='%s')
print("Will process {} BioDots".format(len(coords)))
print('setup swift ...')
#swift = SwiftAPI(dev_port = '/dev/ttyACM0')
#swift = SwiftAPI(filters = {'hwid': 'USB VID:PID=2341:0042'})
swift = SwiftAPI() # default by filters: {'hwid': 'USB VID:PID=2341:0042'}
print('sleep 2 sec ...')
sleep(2)
print('device info: ')
print(swift.get_device_info())
print("Allowing extrusion")
swift.send_cmd_sync("M302 S0")
print('get_position:', swift.get_position())
print('\nset X10 Y193 z50 F1500 ...')
current_z = swift.get_position()[2]
swift.set_position(10, 193, 50, speed=1500, timeout=20)
swift.se
# swift.flush_cmd() # avoid follow 5 command timeout
offset_x = 0
offset_y = 193
total = len(coords)
i = 0
for i in range(50):
print("e={}".format(i / 2))
swift.set_position(10,
193,
50,
continue
# if event.code == 0:# Not good #左边摇杆
# x1 = 5 if event.value < 0 else -5
# pos = swift.get_position()
# x = pos[0]
# swift.set_position(x=x + x1)
# if event.code == 1:
# y1 = 5 if event.value < 0 else -5
# pos = swift.get_position()
# y = pos[1]
# swift.set_position(y=y + y1)
if event.code == 17: #good #左边，上和下
x1 = 5 if event.value < 0 else -5
pos = swift.get_position()
x = pos[0]
swift.set_position(x=x + x1)
if event.code == 16: #左边，左和右
y1 = 5 if event.value < 0 else -5
pos = swift.get_position()
y = pos[1]
swift.set_position(y=y + y1)
if event.code == 308: #右边，上-三角形
z = 35
swift.set_position(z=z)
if event.code == 304: #右边，下-X
z = 2
swift.set_position(z=z)
if event.code == 307: #右边，左-正方形
print('setup swift ...')
# swift = SwiftAPI(dev_port = '/dev/ttyACM0')
# swift = SwiftAPI(filters = {'hwid': 'USB VID:PID=2341:0042'})
swift = SwiftAPI() # default by filters: {'hwid': 'USB VID:PID=2341:0042'}
print('sleep 2 sec ...')
sleep(2)
print('device info: ')
print(swift.get_device_info())
sleep(2)
# swift.reset()
swift.set_position(x=300, wait=True)
sleep(3)
print(swift.get_position())
sleep(3)
swift.set_position(y=0, wait=True)
sleep(3)
# swift.set_position(x=200,y=0, z=45, wait=True)
swift.set_position(z=85, wait=True)
sleep(2)
for x in range(0, 180, 30):
swift.set_wrist(angle=x, wait=True)
sleep(0.2)
swift.set_wrist(angle=90, wait=True)
swift.set_buzzer()
sleep(2)
swift.set_position(x=300, y=0, z=110, wait=True)
sock = socket.socket()
host = '192.168.0.27' #ip of raspberry pi
port = 12345
sock.bind((host, port))
sock.listen(5)
while True:
print('waiting for a connection')
connection, client_address = sock.accept()
try:
print('client connected: {}'.format(client_address))
while True:
data = connection.recv(128).decode()
print('received {}"'.format(str(data)))
if data == 'position':
position = swift.get_position()
positionResponse = 'x: {:03.2f}, y: {:03.2f}, z: {:03.2f}'.format(
position[0], position[1], position[2])
print('sending response: {}'.format(str(positionResponse)))
connection.sendall(bytes(positionResponse, 'utf-8'))
elif data == 'info':
info = str(swift.get_device_info())
print('sending response: {}'.format(str(info)))
connection.sendall(bytes(info, 'utf-8'))
elif data == 'reset':
swift.reset()
connection.sendall(bytes('OK', 'utf-8'))
elif 'move' in data:
command = data.split()
print(str(command))
new_x = command[1]
