Instantly share code, notes, and snippets.

# tomerweller/eduardo.py Created May 12, 2015

 import sys import math import serial __author__ = 'weller' from xml.etree import ElementTree def calc_distance(x1, y1, x2, y2): return math.sqrt(pow(x1 - x2, 2) + pow(y2 - y1, 2)) def sign(x): return math.copysign(1, x) def calc_angle(x1, y1, x2, y2): a = x2 - x1 b = y2 - y1 atan = math.degrees(math.atan(a / b)) if b != 0 else 90 * math.copysign(1, a) if a > 0 > b: return 180 + atan elif a < 0 and b < 0: return -180 + atan elif atan == 0.0 and math.copysign(1, atan) == -1: return 180 else: return atan def get_moves(x, y, x1, y1, x2, y2): moves = [] if calc_distance(x, y, x1, y1) > 0: moves.append([0, x1, y1]) moves.append([1, x2, y2]) return moves def transform(filename): with open(filename, 'r') as content_file: content = content_file.read() tree = ElementTree.fromstring(content) lines = [] for line in tree.findall("{http://www.w3.org/2000/svg}line"): line = { "x1": float(line.attrib["x1"]), "y1": float(line.attrib["y1"]), "x2": float(line.attrib["x2"]), "y2": float(line.attrib["y2"]), } print line["x1"], line["y1"], line["x2"], line["y2"] lines.append(line) x = 0 y = 0 moves = [] while len(lines) > 0: min_distance = sys.maxint closest_line = None closest_moves = [] for line in lines: distance1 = calc_distance(x, y, line["x1"], line["y1"]) distance2 = calc_distance(x, y, line["x2"], line["y2"]) if min(distance1, distance2) < min_distance: closest_line = line if distance1 < distance2: closest_moves = get_moves(x, y, line["x1"], line["y1"], line["x2"], line["y2"]) min_distance = distance1 else: closest_moves = get_moves(x, y, line["x2"], line["y2"], line["x1"], line["y1"]) min_distance = distance2 lines.remove(closest_line) moves.extend(closest_moves) x = moves[-1][1] y = moves[-1][2] print moves x = 0 y = 0 global_angle = 0 commands = [] for move in moves: new_angle = calc_angle(x, y, move[1], move[2]) delta = new_angle - global_angle if delta > 180: delta -= 360 global_angle = new_angle distance = calc_distance(x, y, move[1], move[2]) print "marker: ", move[0], ", turn:", delta, ", distance:", distance left_dir = 1 right_dir = 1 if sign(delta) < 0: right_dir = 0 else: left_dir = 0 commands.extend([move[0], left_dir, right_dir, int(math.floor(abs(delta) * 20 / 9))]) commands.extend([move[0], 1, 1, int(math.floor(distance))]) x = move[1] y = move[2] print commands command_str = "" for command in commands: command_str += str(command) + "," ser = serial.Serial(sys.argv[2], int(sys.argv[3])) ser.write(command_str) print command_str if __name__ == '__main__': print "Start" transform(sys.argv[1]) # print calc_angle(0.0, 0.0, -3.0, 0.0)