def __init__(self):
self.motor_control=MotorController()
# On school network, this function doesn't work
self.get_ip()
self.udp_port = 9001
success=False
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
for i in [0,7,13,17]:
try:
self.udp_port+=i
self.socket.bind((self.ip, self.udp_port))
print "\n"+("*"*50)
print "\nBound To {}:{}\n".format(self.ip,self.udp_port)
print("IP Address: ", self.ip)
print("Port Number: ", self.udp_port)
print ("*"*50)+"\n"
success=True
break
except Exception:
print "Failed To Bind Port: {}".format(self.udp_port)
self.udp_port-=i
if not success:
print "\n"+("*"*50)
print "\n"+("*"*15)+" UDP FAILED TO BIND "+("*"*15)
print ("*"*50)
self.listen()
def __init__(self):
self.board = chess.Board()
self.cnc = MotorController()
self.xa = 2
self.xh = 68.5
self.y1 = 4
self.y8 = 73
self.dx_kill = 3.3
self.dy_kill = 1.7
def __init__(self):
print "Initializing ArmController..."
x, y, z = angles_to_points(DEFAULT_BASE_ANGLE, DEFAULT_J1_ANGLE,
DEFAULT_J2_ANGLE)
print 'Initial coordinates:', x, y, z
self.curr_x = x
self.curr_y = y
self.curr_z = z
self.instructions = []
self.motor_commands = []
self.draw_pause = False
self.draw_abort = False
self.mc = MotorController() # COMMENT WHEN RUNNING ON WINDOWS
def __init__(self):
self.motor_control = MotorController()
# On school network, this function doesn't work
self.get_ip()
self.udp_port = 9001
success = False
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
for i in [0, 7, 13, 17]:
try:
self.udp_port += i
self.socket.bind((self.ip, self.udp_port))
print "\n" + ("*" * 50)
print "\nBound To {}:{}\n".format(self.ip, self.udp_port)
print ("IP Address: ", self.ip)
print ("Port Number: ", self.udp_port)
print ("*" * 50) + "\n"
success = True
break
except Exception:
print "Failed To Bind Port: {}".format(self.udp_port)
self.udp_port -= i
if not success:
print "\n" + ("*" * 50)
print "\n" + ("*" * 15) + " UDP FAILED TO BIND " + ("*" * 15)
print ("*" * 50)
self.listen()
class ArmController:
def __init__(self):
print "Initializing ArmController..."
x, y, z = angles_to_points(DEFAULT_BASE_ANGLE, DEFAULT_J1_ANGLE,
DEFAULT_J2_ANGLE)
print 'Initial coordinates:', x, y, z
self.curr_x = x
self.curr_y = y
self.curr_z = z
self.instructions = []
self.motor_commands = []
self.draw_pause = False
self.draw_abort = False
self.mc = MotorController() # COMMENT WHEN RUNNING ON WINDOWS
# Loads instructions and image metadata from image processing
def load_instructions(self, instructions, img_shape):
self.instructions = instructions
self.img_shape = img_shape
self.img_center_offset_x = (CANVAS_X_PX - img_shape[1]) / 2
self.img_center_offset_y = (CANVAS_Y_PX - img_shape[0]) / 2
print("Loaded {0} drawing instruction(s)".format(len(instructions)))
print self.img_shape
# Processes the loaded instructions, then calls starts running the motors on a new thread
def draw_loaded_instructions(self):
print 'draw_loaded_instructions'
# Convert drawing instructions into motor commands
self.motor_commands = []
for line in self.instructions:
print line.to_string()
self.create_commands_for_line(line)
# Return to starting position
self.return_to_initial_position()
# debug
#for cmd in self.motor_commands:
# print cmd.to_string()
print 'number of motor commands', len(self.motor_commands)
# Load commands into MotorController
# Start draw on seperate thread, so everything else can keep running
self.mc.run_motor_commands(self.motor_commands)
# Update status messages etc
# Parses a line instruction into motor commands
def create_commands_for_line(self, line):
# Move to start of line
x_polar, y_polar = self.canvas_to_polar_coords(line.points[0])
self.add_motor_commands_in_slices(x_polar,
y_polar,
PEN_UP_Z,
no_slice=True)
# Move pen down to canvas
self.pen_down()
# Move pen to each coordinate of the line (connect-the-dots)
for point in line.points:
self.move_to(point)
self.pen_up()
# Adds command to move to canvas-coordinate, while keeping same Z
def move_to(self, coordinate):
# print "move_to coordinate", coordinate
x_polar, y_polar = self.canvas_to_polar_coords(coordinate)
# Skip if already on same pixel
if x_polar == self.curr_x and y_polar == self.curr_y:
return
#print("target", coordinate)
#print("target polar", x_polar, y_polar)
# Get angle of final positions of each arm motor
# angles = points_to_angles([[target_y_mm], [PEN_DOWN_Z]]) # obsolete
# base_angle, j1_angle, j2_angle = point_to_angles(x_polar, y_polar, self.curr_z)
# TODO: Slice movement into smaller segments for straight lines
self.add_motor_commands_in_slices(
x_polar, y_polar, self.curr_z
) # Adds command to move to canvas-coordinate, while keeping same Z
def pen_up(self):
# Skip if pen is already up
if (self.curr_z == PEN_UP_Z):
return
# Get angle of final positions of each arm motor
# Keep same x and y
base_angle, joint_angle1, joint_angle2 = point_to_angles(
self.curr_x, self.curr_y, PEN_UP_Z)
self.curr_z = PEN_UP_Z
# Add to motor command list
new_command = MotorCommand(base_angle, joint_angle1, joint_angle2, 0,
PEN_UP_DOWN_DURATION)
self.motor_commands.append(new_command)
def pen_down(self):
# Skip if pen is already down
if (self.curr_z == PEN_DOWN_Z):
return
# Get angle of final positions of each arm motor
# Keep same x and y
base_angle, joint_angle1, joint_angle2 = point_to_angles(
self.curr_x, self.curr_y, PEN_DOWN_Z)
self.curr_z = PEN_DOWN_Z
# Add to motor command list
new_command = MotorCommand(base_angle, joint_angle1, joint_angle2, 0,
PEN_UP_DOWN_DURATION)
self.motor_commands.append(new_command)
# adds motor commands to move to target xyz
# Updates current xyz
def add_motor_commands_in_slices(self,
target_x,
target_y,
target_z,
no_slice=False):
# starting coords
starting_x = self.curr_x
starting_y = self.curr_y
starting_z = self.curr_z
# How far to move in each direction
dx = target_x - starting_x
dy = target_y - starting_y
dz = target_z - starting_z
move_distance = sqrt(dz**2 + dy**2 + dx**2)
# Optional no slicing, go straight to target
if no_slice:
move_duration = float(move_distance) / ARM_SPEED_MM_PER_S
base_angle, j1_angle, j2_angle = point_to_angles(
target_x, target_y, target_z)
new_command = MotorCommand(base_angle, j1_angle, j2_angle, 0,
move_duration)
self.motor_commands.append(new_command)
self.curr_x = target_x
self.curr_y = target_y
self.curr_z = target_z
return
# Split each movement into equal sized "slices"
slices = int(round(move_distance / MM_PER_MOVESLICE))
if slices < 1:
slices = 1
dx_per_slice = dx / slices
dy_per_slice = dy / slices
dz_per_slice = dz / slices
for i in range(1, slices + 1):
next_x = starting_x + (i * dx_per_slice)
next_y = starting_y + (i * dy_per_slice)
next_z = starting_z + (i * dz_per_slice)
base_angle, j1_angle, j2_angle = point_to_angles(
next_x, next_y, next_z)
# TODO: error check to makes
# TODO: Calculate how fast to move the motors (in seconds)
move_duration = float(move_distance / slices) / ARM_SPEED_MM_PER_S
new_command = MotorCommand(base_angle, j1_angle, j2_angle, 0,
move_duration)
self.motor_commands.append(new_command)
self.curr_x = target_x
self.curr_y = target_y
self.curr_z = target_z
def canvas_to_polar_coords(self, coordinate):
target_x = coordinate[0]
target_y = coordinate[1]
# Convert units to mm
target_x_mm = px_to_mm(target_x)
target_y_mm = px_to_mm(target_y)
# Transform canvas coordinates into polar
# Also tries to center the image if it doesnt fully fit resolution
# Invert Y because openCV has y backwards
polar_x = CANVAS_TO_POLAR_OFFSET_X + (target_x_mm +
self.img_center_offset_x)
polar_y = CANVAS_TO_POLAR_OFFSET_Y - (target_y_mm +
self.img_center_offset_y)
return polar_x, polar_y
def return_to_initial_position(self):
new_command = MotorCommand(DEFAULT_BASE_ANGLE, DEFAULT_J1_ANGLE,
DEFAULT_J2_ANGLE, 0, PEN_UP_DOWN_DURATION)
self.motor_commands.append(new_command)
def testing(self):
# TEST - artificial progress
time_current = 0
while (True):
if (time_current == 5):
print("drawing complete")
return
#pause
if (self.draw_pause):
print("drawing paused")
time.sleep(1)
#unpause
else:
time_current += 1
print("drawing (" + str(time_current) + "/5)")
time.sleep(1)
def draw_image_pause(self):
self.draw_abort = not self.draw_abort
self.mc.drawing_paused = not self.mc.drawing_paused
def draw_image_abort(self):
self.draw_abort = True
self.mc.drawing_aborted = True
def get_drawing_progress(self):
return self.mc.get_drawing_progress()
class NetworkListener:
def __init__(self):
self.motor_control=MotorController()
# On school network, this function doesn't work
self.get_ip()
self.udp_port = 9001
success=False
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
for i in [0,7,13,17]:
try:
self.udp_port+=i
self.socket.bind((self.ip, self.udp_port))
print "\n"+("*"*50)
print "\nBound To {}:{}\n".format(self.ip,self.udp_port)
print("IP Address: ", self.ip)
print("Port Number: ", self.udp_port)
print ("*"*50)+"\n"
success=True
break
except Exception:
print "Failed To Bind Port: {}".format(self.udp_port)
self.udp_port-=i
if not success:
print "\n"+("*"*50)
print "\n"+("*"*15)+" UDP FAILED TO BIND "+("*"*15)
print ("*"*50)
self.listen()
def get_ip(self):
try:
self.ip = socket.gethostbyname(socket.gethostname())
except Exception:
print "\n"+("*"*50)
print ("*"*12)+" Forced To Use 127.0.0.1 "+("*"*13)
print ("*"*50)+"\n"
self.ip = "127.0.0.1"
def listen(self):
try:
while True:
self.data, self.address = self.socket.recvfrom(2048)
try:
movement=json.loads(self.data)
# self.motor_control.run(movement["name"])
if not("confidence" in movement) or movement["confidence"]>.3:
self.motor_control.add_gesture(movement)
print self.motor_control.move()
print movement["name"]
# self.motor_control.add_gesture()
except Exception as e:
print "\nPacket Recieved: \n"+str(e)+"\n"+str(self.data)
# self.motor_control.move()
except KeyboardInterrupt:
self.motor_control.kill()
print "\nClosing Main Thread..."
print "\n"+("*"*50)
print "\nGoodbye"
class Game:
def __init__(self):
self.board = chess.Board()
self.cnc = MotorController()
self.xa = 2
self.xh = 68.5
self.y1 = 4
self.y8 = 73
self.dx_kill = 3.3
self.dy_kill = 1.7
def move_to(self, rank, file, fast, kill=False):
x = file / 7.0 * (self.xh - self.xa) + self.xa
y = rank / 7.0 * (self.y8 - self.y1) + self.y1
if kill:
x = x + self.dx_kill
y = y + self.dy_kill
self.cnc.move_to(x, y, fast)
def move_piece(self, uci):
move = chess.Move.from_uci(uci)
if move not in self.board.legal_moves:
print("Move not legal")
return False
elif self.board.is_en_passant(move):
print("En Passant not supported")
return False
elif self.board.is_castling(move):
print("Castling not supported")
return False
from_file = chess.square_file(move.from_square)
from_rank = chess.square_rank(move.from_square)
to_file = chess.square_file(move.to_square)
to_rank = chess.square_rank(move.to_square)
if self.board.is_capture(move):
self.move_to(to_rank, to_file, True, True)
self.cnc.kill_piece()
self.move_to(from_rank, from_file, True)
self.cnc.engage_magnet(True)
if from_rank == to_rank or from_file == to_file or abs(
to_file - from_file) == abs(to_rank - from_rank):
# if moving horizontally, vertically, or diagonally, go straight there
self.move_to(to_rank, to_file, False)
elif abs(to_rank - from_rank) == 1 and abs(to_file - from_file) == 2:
#knight move 1 rank 2 files
self.move_to(0.5 * (from_rank + to_rank), from_file, False)
self.move_to(0.5 * (from_rank + to_rank), to_file, False)
self.move_to(to_rank, to_file, False)
elif abs(to_file - from_file) == 1 and abs(to_rank - from_rank) == 2:
#knight move 1 file 2 ranks
self.move_to(from_rank, (from_file + to_file) * 0.5, False)
self.move_to(to_rank, (from_file + to_file) * 0.5, False)
self.move_to(to_rank, to_file, False)
else:
print("unexpected error, impossible move")
raise
self.cnc.engage_magnet(False)
self.board.push(move)
return True
def test(self):
# test_moves = ['d2d4', 'c7c6', 'g1f3', 'e7e6', 'c1f4', 'c6c5', 'e2e3', 'd7d5', 'd1d3', 'c5d4', 'e3d4']
test_moves = ['d2d4', 'e7e5', 'd4e5']
for move in test_moves:
self.move_piece(move)
time.sleep(5)
class NetworkListener:
def __init__(self):
self.motor_control = MotorController()
# On school network, this function doesn't work
self.get_ip()
self.udp_port = 9001
success = False
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
for i in [0, 7, 13, 17]:
try:
self.udp_port += i
self.socket.bind((self.ip, self.udp_port))
print "\n" + ("*" * 50)
print "\nBound To {}:{}\n".format(self.ip, self.udp_port)
print ("IP Address: ", self.ip)
print ("Port Number: ", self.udp_port)
print ("*" * 50) + "\n"
success = True
break
except Exception:
print "Failed To Bind Port: {}".format(self.udp_port)
self.udp_port -= i
if not success:
print "\n" + ("*" * 50)
print "\n" + ("*" * 15) + " UDP FAILED TO BIND " + ("*" * 15)
print ("*" * 50)
self.listen()
def get_ip(self):
try:
self.ip = socket.gethostbyname(socket.gethostname())
except Exception:
print "\n" + ("*" * 50)
print ("*" * 12) + " Forced To Use 127.0.0.1 " + ("*" * 13)
print ("*" * 50) + "\n"
self.ip = "127.0.0.1"
def listen(self):
try:
while True:
self.data, self.address = self.socket.recvfrom(2048)
try:
movement = json.loads(self.data)
# self.motor_control.run(movement["name"])
if not ("confidence" in movement) or movement["confidence"] > 0.3:
self.motor_control.add_gesture(movement)
print self.motor_control.move()
print movement["name"]
# self.motor_control.add_gesture()
except Exception as e:
print "\nPacket Recieved: \n" + str(e) + "\n" + str(self.data)
# self.motor_control.move()
except KeyboardInterrupt:
self.motor_control.kill()
print "\nClosing Main Thread..."
print "\n" + ("*" * 50)
print "\nGoodbye"
