Skip to content

Instantly share code, notes, and snippets.

@tomerweller
Created May 12, 2015 16:02
Show Gist options
  • Save tomerweller/517f738c91d92897dfab to your computer and use it in GitHub Desktop.
Save tomerweller/517f738c91d92897dfab to your computer and use it in GitHub Desktop.
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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment