Skip to content

Commit

Permalink
Code cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@alver
alver committed Aug 15, 2016
1 parent ab1e28b commit 08315cf
Showing 1 changed file with 39 additions and 39 deletions.
78 changes: 39 additions & 39 deletions writer.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,12 @@

from svg.parser import parse_path
from svg.path import Line

class mymotor(Motor):
def stop(self, stop_command='coast'):
self.stop_command = stop_command
self.command = "stop"

def reset_position(self, value = 0):
self.stop()
iter = 1
Expand All @@ -22,7 +22,7 @@ def reset_position(self, value = 0):
except:
print "impossible to fix position, attempt",iter-1,"on 10."
time.sleep(0.05)

def rotate_forever(self, speed=480, regulate='on', stop_command='brake'):
self.stop_command = stop_command
if regulate=='on':
Expand All @@ -43,7 +43,7 @@ def goto_position(self, position, speed=480, up=0, down=0, regulate='on', stop_c
self.position_sp = position
sign = math.copysign(1, self.position - position)
self.command = 'run-to-abs-pos'

if (wait):
new_pos = self.position
nb_same = 0
Expand All @@ -63,24 +63,24 @@ def goto_position(self, position, speed=480, up=0, down=0, regulate='on', stop_c


class Writer():

def __init__(self, calibrate=True):
self.mot_A = mymotor(OUTPUT_C)
self.mot_B = mymotor(OUTPUT_A)

self.mot_lift = mymotor(OUTPUT_B)

self.touch_A = TouchSensor(INPUT_3)
self.touch_B = TouchSensor(INPUT_2)

if (calibrate):
self.calibrate()
self.pen_up()

def pen_up (self):
self.mot_lift.goto_position(40, 30, regulate = 'off', stop_command='brake', wait = 1)
time.sleep(0.1)

def pen_down(self):
self.mot_lift.goto_position(14, 30, regulate = 'off', stop_command='brake', wait = 1)
time.sleep(0.1)
Expand All @@ -98,18 +98,18 @@ def calibrate (self):
time.sleep(0.1)
self.mot_lift.reset_position()
time.sleep(1)

self.pen_up()

self.mot_A.reset_position()
self.mot_B.reset_position()

if (self.touch_A.value()):
self.mot_A.goto_position(-200, speed=400, regulate='on', stop_command='coast', wait=1)
if (self.touch_B.value()):
self.mot_B.goto_position(200, speed=400, regulate='on', stop_command='coast', wait=1)
self.mot_B.rotate_forever(speed=-50, regulate='off')
self.mot_A.rotate_forever(speed=50, regulate='off')
self.mot_B.rotate_forever(speed=-50, regulate='off')
self.mot_A.rotate_forever(speed=50, regulate='off')
stop_A = stop_B = False
start = time.time()
while True:
Expand Down Expand Up @@ -140,7 +140,7 @@ def calibrate (self):
self.mot_B.stop()
self.mot_A.reset_position()
self.mot_B.reset_position()

# All coordinates are in Lego distance (1 = distance between two holes center)
# Coordinates of gear centre A
xA, yA = 0.,0.
Expand Down Expand Up @@ -193,9 +193,9 @@ def coordinates_to_angles (xE, yE):
xIB = xIB2
yIB = yIB2
except:
return None
return None
alpha = 180. - 360 * math.acos((xIA-Writer.xA)/Writer.r2) / (2.*math.pi)
beta = 360. * math.acos((xIB-Writer.xB)/Writer.r2) / (2.*math.pi)
beta = 360. * math.acos((xIB-Writer.xB)/Writer.r2) / (2.*math.pi)
return (alpha, beta)

## converts coordinates x,y into motor position
Expand All @@ -220,18 +220,18 @@ def angles_to_coordinates (alpha, beta):
xE = xE2
yE = yE2
return xE, yE

## Converts motor position to coordinates
@staticmethod
def motorpos_to_coordinates (pos1, pos2):
def pos_to_angle (pos):
#0 = 14
#-2970 = 90
return 14. + pos * (90.-14) / 2970.

(alpha, beta) = (pos_to_angle(pos1), pos_to_angle(-pos2))
return Writer.angles_to_coordinates (alpha, beta)

@staticmethod
def get_angle (xA, yA, xB, yB, xC, yC):
ab2 = (xB-xA)*(xB-xA) + (yB-yA)*(yB-yA)
Expand All @@ -256,9 +256,9 @@ def set_speed_to_coordinates (self,x,y,max_speed,initx=None,inity=None,brake=0.)

nextx = myx + (x - myx) / (dist * 100.)
nexty = myy + (y - myy) / (dist * 100.)

next_posB, next_posA = Writer.coordinates_to_motorpos (nextx, nexty)

speed = max_speed
slow_down_dist = (max_speed / 50.)
if (dist < slow_down_dist):
Expand All @@ -272,11 +272,11 @@ def set_speed_to_coordinates (self,x,y,max_speed,initx=None,inity=None,brake=0.)
else:
speedA = speed
speedB = abs(speedA / distA * distB)

self.mot_B.rotate_forever((math.copysign(speedB, distB)), regulate='off')
self.mot_A.rotate_forever((math.copysign(speedA, distA)), regulate='off')
return 1

def goto_point (self, x,y, brake=1., last_x=None, last_y=None, max_speed=70.):
if (last_x == None or last_y == None):
initposB, initposA = self.mot_B.position, self.mot_A.position
Expand All @@ -291,7 +291,7 @@ def goto_point (self, x,y, brake=1., last_x=None, last_y=None, max_speed=70.):
if brake == 1:
self.mot_B.stop(stop_command='brake')
self.mot_A.stop(stop_command='brake')

def follow_path (self, list_points, max_speed=70):
pen_change = False
lastx = lasty = None
Expand Down Expand Up @@ -323,14 +323,14 @@ def follow_path (self, list_points, max_speed=70):
lastx, lasty = x, y
self.mot_A.stop()
self.mot_B.stop()

def read_svg (self, image_file):
# Open simple svg created from template.svg with only paths and no transform.
# To remove transformations from svg and convert objects to path, use:
# inkscape --verb=EditSelectAll --verb=ObjectToPath --verb=SelectionUnGroup --verb=FileSave --verb=FileClose --verb=FileQuit my_image.svg

from xml.dom import minidom

def svg_point_to_coord (svg_point):
scale = 10.
ciblex = svg_point.real/scale
Expand All @@ -340,17 +340,17 @@ def feq(a,b):
if abs(a-b)<0.0001:
return 1
else:
return 0
return 0

xmldoc = minidom.parse(image_file)

itemlist = xmldoc.getElementsByTagName('path')
itemlist = xmldoc.getElementsByTagName('path')
try:
itemlist = filter(lambda x: x.attributes['id'].value != "borders", itemlist)
except:
pass
path = [s.attributes['d'].value for s in itemlist]

list_points = []
actual = (0+0j)
for p_ in path:
Expand All @@ -373,7 +373,7 @@ def feq(a,b):
actual = end
list_points.append(0)
return list_points


def fit_path (self, points):
def get_bounding_box (points):
Expand Down Expand Up @@ -406,12 +406,12 @@ def get_circles (r1, r2, xA, yA, xB, yB):
def drange(start, stop, step):
r = start
while r < stop:
yield r
r += step
yield r
r += step

(bbox_x, bbox_y, bbox_w, bbox_h) = get_bounding_box (points)
(left_top, left_bottom) = get_circles (Writer.r1, Writer.r2, Writer.xA, Writer.yA, Writer.xB, Writer.yB)

min_x = max(left_top[0] - left_top[2] , left_bottom[0] - left_bottom[2] )
best_fit, best_fit_x, best_fit_y, best_scale = 10000, 0,0,0
mx = (Writer.xB + Writer.xA)/2.
Expand All @@ -421,20 +421,20 @@ def drange(start, stop, step):
if (y1> y2):
if abs(((mx-x)*2)/(y1-y2) - (bbox_w/bbox_h)) < best_fit:
best_fit, best_fit_x, best_fit_y, best_scale = abs((mx-x)*2)/(y1-y2) - (bbox_w/bbox_h), x, y2, (mx-x)*2 / bbox_w

new_points = []
for point in points:
if type(point) is int:
new_points.append (point)
else:
new_points.append(((point[0]-bbox_x)*best_scale + best_fit_x,(point[1]-bbox_y)*best_scale + best_fit_y))
return new_points


def draw_image (self, image_file = 'images/drawing.svg', max_speed=70.):
list_points = self.fit_path (self.read_svg (image_file))
self.follow_path(list_points, max_speed=max_speed)

def follow_mouse (self, path="/dev/input/by-id/usb-0461_USB_Optical_Mouse-event-mouse"):
if not os.path.exists(path):
return
Expand All @@ -454,7 +454,7 @@ def follow_mouse (self, path="/dev/input/by-id/usb-0461_USB_Optical_Mouse-event-
ciblex = startx-x
cibley = starty+y
print ciblex, cibley

if ("BTN_LEFT" in dev.buttons.keys()):
if (pen_up and dev.buttons["BTN_LEFT"]):
pen_up = False
Expand Down

0 comments on commit 08315cf

Please sign in to comment.