eduardo.py

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)