summaryrefslogtreecommitdiffhomepage
path: root/host/inter
diff options
context:
space:
mode:
authorNicolas Schodet2009-02-09 20:05:30 +0100
committerNicolas Schodet2009-02-09 20:05:30 +0100
commit2e95e3a33bcb34aeec66551503c692c1cb80ab61 (patch)
tree6b763ee0d1bf458da0ced13f44535b42f3b8efb8 /host/inter
parent552b28da98a387d13aea7403a7e72732a913277d (diff)
* all python:
- changed tabs to spaces.
Diffstat (limited to 'host/inter')
-rw-r--r--host/inter/dist_sensor.py88
-rw-r--r--host/inter/drawable.py176
-rw-r--r--host/inter/inter.py380
-rw-r--r--host/inter/inter_node.py266
-rw-r--r--host/inter/path.py12
-rw-r--r--host/inter/test/test_drawable.py48
-rw-r--r--host/inter/trans_matrix.py226
7 files changed, 598 insertions, 598 deletions
diff --git a/host/inter/dist_sensor.py b/host/inter/dist_sensor.py
index 54c61e6d..b1bd0d0d 100644
--- a/host/inter/dist_sensor.py
+++ b/host/inter/dist_sensor.py
@@ -32,51 +32,51 @@ class DistSensor (Drawable):
"""A distance sensor."""
def __init__ (self, onto, pos, angle, range):
- Drawable.__init__ (self, onto)
- self.pos = pos
- self.angle = angle
- self.range = range
- self.target = (pos[0] + cos (angle) * range,
- pos[1] + sin (angle) * range)
- self.obstacles = [ ]
- self.distance = None
- self.hide = False
+ Drawable.__init__ (self, onto)
+ self.pos = pos
+ self.angle = angle
+ self.range = range
+ self.target = (pos[0] + cos (angle) * range,
+ pos[1] + sin (angle) * range)
+ self.obstacles = [ ]
+ self.distance = None
+ self.hide = False
def compute (self):
- # Could do better by trans_applying until on the same drawable as the
- # obstacle.
- pos = self.onto.trans_apply (self.pos)
- target = self.onto.trans_apply (self.target)
- n = ((target[0] - pos[0]) / self.range,
- (target[1] - pos[1]) / self.range)
- self.distance = None
- for o in self.obstacles:
- # Does the line intersect with the obstacle?
- ao = (o.pos[0] - pos[0], o.pos[1] - pos[1])
- doc = abs (ao[0] * -n[1] + ao[1] * n[0])
- if doc < o.radius:
- # Does the segment intersect?
- m = ao[0] * n[0] + ao[1] * n[1]
- f = sqrt (o.radius ** 2 - doc ** 2)
- if m - f > 0 and m - f < self.range:
- d = m - f
- if self.distance is None or self.distance > d:
- self.distance = d
- elif m + f > 0 and m + f < self.range:
- d = m + f
- if self.distance is None or self.distance > d:
- self.distance = d
- return self.distance
+ # Could do better by trans_applying until on the same drawable as the
+ # obstacle.
+ pos = self.onto.trans_apply (self.pos)
+ target = self.onto.trans_apply (self.target)
+ n = ((target[0] - pos[0]) / self.range,
+ (target[1] - pos[1]) / self.range)
+ self.distance = None
+ for o in self.obstacles:
+ # Does the line intersect with the obstacle?
+ ao = (o.pos[0] - pos[0], o.pos[1] - pos[1])
+ doc = abs (ao[0] * -n[1] + ao[1] * n[0])
+ if doc < o.radius:
+ # Does the segment intersect?
+ m = ao[0] * n[0] + ao[1] * n[1]
+ f = sqrt (o.radius ** 2 - doc ** 2)
+ if m - f > 0 and m - f < self.range:
+ d = m - f
+ if self.distance is None or self.distance > d:
+ self.distance = d
+ elif m + f > 0 and m + f < self.range:
+ d = m + f
+ if self.distance is None or self.distance > d:
+ self.distance = d
+ return self.distance
def draw (self):
- self.compute ()
- self.reset ()
- if self.hide:
- return
- if self.distance is None:
- self.draw_line (self.pos, self.target, fill = 'blue', arrow = LAST)
- else:
- inter = (self.pos[0] + cos (self.angle) * self.distance,
- self.pos[1] + sin (self.angle) * self.distance)
- self.draw_line (self.pos, inter, fill = 'red', arrow = LAST)
- self.draw_line (inter, self.target, fill = 'blue', arrow = LAST)
+ self.compute ()
+ self.reset ()
+ if self.hide:
+ return
+ if self.distance is None:
+ self.draw_line (self.pos, self.target, fill = 'blue', arrow = LAST)
+ else:
+ inter = (self.pos[0] + cos (self.angle) * self.distance,
+ self.pos[1] + sin (self.angle) * self.distance)
+ self.draw_line (self.pos, inter, fill = 'red', arrow = LAST)
+ self.draw_line (inter, self.target, fill = 'blue', arrow = LAST)
diff --git a/host/inter/drawable.py b/host/inter/drawable.py
index 26628f40..81331548 100644
--- a/host/inter/drawable.py
+++ b/host/inter/drawable.py
@@ -32,78 +32,78 @@ class Drawable:
"""Define a drawable area with embedded transformations."""
def __init__ (self, onto):
- """Initialise the drawable."""
- self.onto = onto
- self.trans_matrix = trans_matrix.TransMatrix ()
- self.items = [ ]
- self.trans_apply = self.trans_matrix.apply
- self.trans_apply_angle = self.trans_matrix.apply_angle
- self.trans_apply_distance = self.trans_matrix.apply_distance
- self.trans_rotate = self.trans_matrix.rotate
- self.trans_translate = self.trans_matrix.translate
- self.trans_scale = self.trans_matrix.scale
+ """Initialise the drawable."""
+ self.onto = onto
+ self.trans_matrix = trans_matrix.TransMatrix ()
+ self.items = [ ]
+ self.trans_apply = self.trans_matrix.apply
+ self.trans_apply_angle = self.trans_matrix.apply_angle
+ self.trans_apply_distance = self.trans_matrix.apply_distance
+ self.trans_rotate = self.trans_matrix.rotate
+ self.trans_translate = self.trans_matrix.translate
+ self.trans_scale = self.trans_matrix.scale
def __draw_rectangle (self, p1, p2, **kw):
- if 'outline' not in kw:
- kw = kw.copy ()
- kw['outline'] = 'black'
- p = self.trans_apply (p1, (p2[0], p1[1]), p2, (p1[0], p2[1]))
- return self.onto.__draw_polygon (*p, **kw)
+ if 'outline' not in kw:
+ kw = kw.copy ()
+ kw['outline'] = 'black'
+ p = self.trans_apply (p1, (p2[0], p1[1]), p2, (p1[0], p2[1]))
+ return self.onto.__draw_polygon (*p, **kw)
def __draw_line (self, *p, **kw):
- p = self.trans_apply (*p)
- return self.onto.__draw_line (*p, **kw)
+ p = self.trans_apply (*p)
+ return self.onto.__draw_line (*p, **kw)
def __draw_polygon (self, *p, **kw):
- p = self.trans_apply (*p)
- return self.onto.__draw_polygon (*p, **kw)
+ p = self.trans_apply (*p)
+ return self.onto.__draw_polygon (*p, **kw)
def __draw_circle (self, p, r, **kw):
- p = self.trans_apply (p)
- r = self.trans_apply_distance (r)
- return self.onto.__draw_circle (p, r, **kw)
+ p = self.trans_apply (p)
+ r = self.trans_apply_distance (r)
+ return self.onto.__draw_circle (p, r, **kw)
def __draw_arc (self, p, r, **kw):
- p = self.trans_apply (p)
- r = self.trans_apply_distance (r)
- if 'start' in kw:
- kw = kw.copy ()
- kw['start'] = self.trans_apply_angle (kw['start'])
- import math
- return self.onto.__draw_arc (p, r, **kw)
+ p = self.trans_apply (p)
+ r = self.trans_apply_distance (r)
+ if 'start' in kw:
+ kw = kw.copy ()
+ kw['start'] = self.trans_apply_angle (kw['start'])
+ import math
+ return self.onto.__draw_arc (p, r, **kw)
def draw_rectangle (self, *p, **kw):
- """Draw a rectangle."""
- self.items.append (self.__draw_rectangle (*p, **kw))
+ """Draw a rectangle."""
+ self.items.append (self.__draw_rectangle (*p, **kw))
def draw_line (self, *p, **kw):
- """Draw a line."""
- self.items.append (self.__draw_line (*p, **kw))
+ """Draw a line."""
+ self.items.append (self.__draw_line (*p, **kw))
def draw_polygon (self, *p, **kw):
- """Draw a line."""
- self.items.append (self.__draw_polygon (*p, **kw))
+ """Draw a line."""
+ self.items.append (self.__draw_polygon (*p, **kw))
def draw_circle (self, p, r, **kw):
- """Draw a circle of the given radius centered on p."""
- self.items.append (self.__draw_circle (p, r, **kw))
+ """Draw a circle of the given radius centered on p."""
+ self.items.append (self.__draw_circle (p, r, **kw))
def draw_arc (self, p, r, **kw):
- """Draw a arc of the given radius centered on p."""
- self.items.append (self.__draw_arc (p, r, **kw))
+ """Draw a arc of the given radius centered on p."""
+ self.items.append (self.__draw_arc (p, r, **kw))
def trans_reset (self):
- """Reset transformations."""
- self.trans_matrix.identity ()
+ """Reset transformations."""
+ self.trans_matrix.identity ()
def reset (self):
- self.__delete (*self.items)
- self.items = [ ]
- self.trans_reset ()
+ self.__delete (*self.items)
+ self.items = [ ]
+ self.trans_reset ()
def __delete (self, *list):
- """Delete a list of items."""
- self.onto.__delete (*list)
+ """Delete a list of items."""
+ self.onto.__delete (*list)
class DrawableCanvas(Tkinter.Canvas):
@@ -111,62 +111,62 @@ class DrawableCanvas(Tkinter.Canvas):
the draw method."""
def __init__ (self, width, height, xorigin, yorigin, master = None, **kw):
- """Initialise a DrawableCanvas. The width and height parameters
- define the requested drawable area virtual size. The xorigin and
- yorigin parameters define origin of the virtual coordinates relative
- to the drawable center."""
- Tkinter.Canvas.__init__ (self, master, **kw)
- self.__width = width
- self.__height = height
- self.__xorigin = xorigin
- self.__yorigin = yorigin
- self.bind ('<Configure>', self.__resize)
+ """Initialise a DrawableCanvas. The width and height parameters
+ define the requested drawable area virtual size. The xorigin and
+ yorigin parameters define origin of the virtual coordinates relative
+ to the drawable center."""
+ Tkinter.Canvas.__init__ (self, master, **kw)
+ self.__width = width
+ self.__height = height
+ self.__xorigin = xorigin
+ self.__yorigin = yorigin
+ self.bind ('<Configure>', self.__resize)
def __resize (self, ev):
- # Compute new scale.
- w, h = float (ev.width), float (ev.height)
- self.__scale = min (w / self.__width, h / self.__height)
- self.__xoffset = w / 2 + self.__xorigin * self.__scale
- self.__yoffset = h / 2 - self.__yorigin * self.__scale
- # Redraw.
- self.draw ()
+ # Compute new scale.
+ w, h = float (ev.width), float (ev.height)
+ self.__scale = min (w / self.__width, h / self.__height)
+ self.__xoffset = w / 2 + self.__xorigin * self.__scale
+ self.__yoffset = h / 2 - self.__yorigin * self.__scale
+ # Redraw.
+ self.draw ()
def _Drawable__draw_line (self, *p, **kw):
- p = self.__coord (*p)
- return self.create_line (*p, **kw)
+ p = self.__coord (*p)
+ return self.create_line (*p, **kw)
def _Drawable__draw_polygon (self, *p, **kw):
- p = self.__coord (*p)
- return self.create_polygon (*p, **kw)
+ p = self.__coord (*p)
+ return self.create_polygon (*p, **kw)
def _Drawable__draw_circle (self, p, r, **kw):
- p, = self.__coord (p)
- r = r * self.__scale
- p1 = (p[0] - r, p[1] - r)
- p2 = (p[0] + r, p[1] + r)
- return self.create_oval (p1, p2, **kw)
+ p, = self.__coord (p)
+ r = r * self.__scale
+ p1 = (p[0] - r, p[1] - r)
+ p2 = (p[0] + r, p[1] + r)
+ return self.create_oval (p1, p2, **kw)
def _Drawable__draw_arc (self, p, r, **kw):
- p, = self.__coord (p)
- r = r * self.__scale
- p1 = (p[0] - r, p[1] - r)
- p2 = (p[0] + r, p[1] + r)
- for k in ('start', 'extent'):
- if k in kw:
- kw = kw.copy ()
- kw[k] = degrees (kw[k])
- return self.create_arc (p1, p2, **kw)
+ p, = self.__coord (p)
+ r = r * self.__scale
+ p1 = (p[0] - r, p[1] - r)
+ p2 = (p[0] + r, p[1] + r)
+ for k in ('start', 'extent'):
+ if k in kw:
+ kw = kw.copy ()
+ kw[k] = degrees (kw[k])
+ return self.create_arc (p1, p2, **kw)
def _Drawable__delete (self, *list):
- self.delete (*list)
+ self.delete (*list)
def __coord (self, *args):
- return [ (i[0] * self.__scale + self.__xoffset,
- -i[1] * self.__scale + self.__yoffset) for i in args ]
+ return [ (i[0] * self.__scale + self.__xoffset,
+ -i[1] * self.__scale + self.__yoffset) for i in args ]
def screen_coord (self, screen):
- """Return drawable coordinates corresponding to the given screen
- coordinates."""
- return ((self.canvasx (screen[0]) - self.__xoffset) / self.__scale,
- -(self.canvasy (screen[1]) - self.__yoffset) / self.__scale)
+ """Return drawable coordinates corresponding to the given screen
+ coordinates."""
+ return ((self.canvasx (screen[0]) - self.__xoffset) / self.__scale,
+ -(self.canvasy (screen[1]) - self.__yoffset) / self.__scale)
diff --git a/host/inter/inter.py b/host/inter/inter.py
index cde1bd73..4c19e5c8 100644
--- a/host/inter/inter.py
+++ b/host/inter/inter.py
@@ -31,105 +31,105 @@ class Robot (Drawable):
"""The robot."""
def __init__ (self, onto):
- Drawable.__init__ (self, onto)
- self.drawn = [ ]
+ Drawable.__init__ (self, onto)
+ self.drawn = [ ]
def draw (self):
- self.reset ()
- self.trans_rotate (self.angle)
- self.trans_translate (self.pos)
- self.draw_polygon ((115, 30), (170, 85), (150, 127), (130, 145),
- (-25, 200), (-70, 200), (-70, -200), (-25, -200),
- (130, -145), (150, -127), (170, -85), (115, -30))
- axes_fill = '#404040'
- self.draw_line ((-70, 0), (170, 0), fill = axes_fill, arrow = LAST)
- f = 142 + 2 * 31.5 - 13.5
- wr = 63 / 2
- self.draw_line ((0, +f / 2), (0, -f / 2), fill = axes_fill)
- self.draw_line ((-wr, f / 2), (+wr, f / 2), fill = axes_fill)
- self.draw_line ((-wr, -f / 2), (+wr, -f / 2), fill = axes_fill)
- for i in self.drawn:
- i.draw ()
+ self.reset ()
+ self.trans_rotate (self.angle)
+ self.trans_translate (self.pos)
+ self.draw_polygon ((115, 30), (170, 85), (150, 127), (130, 145),
+ (-25, 200), (-70, 200), (-70, -200), (-25, -200),
+ (130, -145), (150, -127), (170, -85), (115, -30))
+ axes_fill = '#404040'
+ self.draw_line ((-70, 0), (170, 0), fill = axes_fill, arrow = LAST)
+ f = 142 + 2 * 31.5 - 13.5
+ wr = 63 / 2
+ self.draw_line ((0, +f / 2), (0, -f / 2), fill = axes_fill)
+ self.draw_line ((-wr, f / 2), (+wr, f / 2), fill = axes_fill)
+ self.draw_line ((-wr, -f / 2), (+wr, -f / 2), fill = axes_fill)
+ for i in self.drawn:
+ i.draw ()
class Obstacle (Drawable):
"""An obstacle."""
def __init__ (self, onto, pos, radius):
- Drawable.__init__ (self, onto)
- self.pos = pos
- self.radius = radius
+ Drawable.__init__ (self, onto)
+ self.pos = pos
+ self.radius = radius
def draw (self):
- self.reset ()
- self.trans_translate (self.pos)
- self.draw_circle ((0, 0), self.radius, fill = '#31aa23')
- self.draw_circle ((0, 0), self.radius + 250, outlinestipple = 'gray25')
+ self.reset ()
+ self.trans_translate (self.pos)
+ self.draw_circle ((0, 0), self.radius, fill = '#31aa23')
+ self.draw_circle ((0, 0), self.radius + 250, outlinestipple = 'gray25')
class Table (Drawable):
"""The table and its elements."""
def draw (self):
- # Redraw.
- self.reset ()
- # Table.
- self.draw_rectangle ((-22, -22 - 80), (3000 / 2, 2100 + 22), fill = '#ff1f1f')
- self.draw_rectangle ((3000 / 2, -22 - 80), (3000 + 22, 2100 + 22), fill = '#201fff')
- self.draw_rectangle ((0, 0), (3000, 2100), fill = '#a49d8b')
- self.draw_rectangle ((0, -22 - 80), (3000, -22), fill = '#a49d8b')
- self.draw_rectangle ((0, 2100 - 500), (500, 2100), fill = '#201fff')
- self.draw_rectangle ((3000 - 500, 2100 - 500), (3000, 2100), fill = '#ff1f1f')
- self.draw_line ((3000 / 2, -22 - 80), (3000 / 2, 2100 + 22))
- # Axes.
- self.draw_line ((0, 200), (0, 0), (200, 0), arrow = BOTH)
- # Beacons and baskets.
- self.draw_rectangle ((-22, 2100), (-22 - 80, 2100 + 80), fill = '#5b5b5d')
- self.draw_rectangle ((-22, 1050 - 40), (-22 - 80, 1050 + 40), fill = '#5b5b5d')
- self.draw_rectangle ((-22, 500), (-22 - 80, 500 + 80), fill = '#5b5b5d')
- self.draw_rectangle ((-22, -80), (-22 - 80, 0), fill = '#5b5b5d')
- self.draw_rectangle ((-22, 0), (-22 - 80, 500), fill = '#5b5b5d')
- self.draw_rectangle ((-22 - 80 - 250, 0), (-22 - 80, 500), fill = '#6d6dad', stipple = 'gray75')
- self.draw_rectangle ((3000 + 22, 2100), (3000 + 22 + 80, 2100 + 80), fill = '#5b5b5d')
- self.draw_rectangle ((3000 + 22, 1050 - 40), (3000 + 22 + 80, 1050 + 40), fill = '#5b5b5d')
- self.draw_rectangle ((3000 + 22, 500), (3000 + 22 + 80, 500 + 80), fill = '#5b5b5d')
- self.draw_rectangle ((3000 + 22, -80), (3000 + 22 + 80, 0), fill = '#5b5b5d')
- self.draw_rectangle ((3000 + 22, 0), (3000 + 22 + 80, 500), fill = '#5b5b5d')
- self.draw_rectangle ((3000 + 22 + 80 + 250, 0), (3000 + 22 + 80, 500), fill = '#6d6dad', stipple = 'gray75')
- # Vertical dispensers.
- self.draw_rectangle ((-22, 2100 - 750 - 85 / 2), (0, 2100 - 750 + 85 / 2), fill = '#5b5b5b')
- self.draw_circle ((40, 2100 - 750), 40)
- self.draw_rectangle ((700 - 85 / 2, 2100), (700 + 85 / 2, 2100 + 22), fill = '#5b5b5b')
- self.draw_circle ((700, 2100 - 40), 40)
- self.draw_rectangle ((3000 + 22, 2100 - 750 - 85 / 2), (3000, 2100 - 750 + 85 / 2), fill = '#5b5b5b')
- self.draw_circle ((3000 - 40, 2100 - 750), 40)
- self.draw_rectangle ((3000 - 700 + 85 / 2, 2100), (3000 - 700 - 85 / 2, 2100 + 22), fill = '#5b5b5b')
- self.draw_circle ((3000 - 700, 2100 - 40), 40)
- # Horizontal dispenser.
- self.draw_rectangle ((3000 / 2 - 924 / 2, 2100 + 22), (3000 / 2 + 924 / 2, 2100 + 22 + 80 + 22), fill = '#5b5b5b')
- self.draw_rectangle ((3000 / 2 - 924 / 2 + 22, 2100 + 22), (3000 / 2 + 924 / 2 - 22, 2100 + 22 + 80), fill = '#5b5b5b')
- self.draw_rectangle ((3000 / 2 - 880 / 2 - 35 - 60, 2100), (3000 / 2 - 880 / 2 - 35, 2100 + 44), fill = '#5b5b5b')
- self.draw_rectangle ((3000 / 2 + 880 / 2 + 35 + 60, 2100), (3000 / 2 + 880 / 2 + 35, 2100 + 44), fill = '#5b5b5b')
- # Balls.
- balls = [ (800, 200, 'rb'), (800, 400, 'RB'), (800, 600, 'ww'),
- (1300, 200, 'rb'), (1300, 400, 'rb'), (1300, 600, 'ww'),
- (520, 800, 'WW'), (700, 40, 'RB'), (40, 750, 'WW'),
- (450, 1120, 'ww'), (750, 1070, 'ww'), (1050, 1020, 'ww'),
- (1500 - 72 / 2, -22 - 80 / 2, 'BR'),
- (1500 - 72 / 2 - 1 * 73, -22 - 80 / 2, 'RB'),
- (1500 - 72 / 2 - 2 * 73, -22 - 80 / 2, 'BR'),
- (1500 - 72 / 2 - 3 * 73, -22 - 80 / 2, 'RB'),
- (1500 - 72 / 2 - 4 * 73, -22 - 80 / 2, 'BR'),
- (1500 - 72 / 2 - 5 * 73, -22 - 80 / 2, 'RB'),
- (1500, 1000, 'W'),
- ]
- balls_config = { 'r': { 'outline': '#bf4141' }, 'R': { 'fill': '#bf4141' },
- 'b': { 'outline': '#4241bf' }, 'B': { 'fill': '#4241bf' },
- 'w': { 'outline': '#bfbfbf' }, 'W': { 'fill': '#bfbfbf' } }
- for b in balls:
- self.draw_circle ((3000 - b[0], 2100 - b[1]), 72 / 2,
- **balls_config[b[2][0]])
- if len (b[2]) > 1:
- self.draw_circle ((b[0], 2100 - b[1]), 72 / 2,
- **balls_config[b[2][1]])
+ # Redraw.
+ self.reset ()
+ # Table.
+ self.draw_rectangle ((-22, -22 - 80), (3000 / 2, 2100 + 22), fill = '#ff1f1f')
+ self.draw_rectangle ((3000 / 2, -22 - 80), (3000 + 22, 2100 + 22), fill = '#201fff')
+ self.draw_rectangle ((0, 0), (3000, 2100), fill = '#a49d8b')
+ self.draw_rectangle ((0, -22 - 80), (3000, -22), fill = '#a49d8b')
+ self.draw_rectangle ((0, 2100 - 500), (500, 2100), fill = '#201fff')
+ self.draw_rectangle ((3000 - 500, 2100 - 500), (3000, 2100), fill = '#ff1f1f')
+ self.draw_line ((3000 / 2, -22 - 80), (3000 / 2, 2100 + 22))
+ # Axes.
+ self.draw_line ((0, 200), (0, 0), (200, 0), arrow = BOTH)
+ # Beacons and baskets.
+ self.draw_rectangle ((-22, 2100), (-22 - 80, 2100 + 80), fill = '#5b5b5d')
+ self.draw_rectangle ((-22, 1050 - 40), (-22 - 80, 1050 + 40), fill = '#5b5b5d')
+ self.draw_rectangle ((-22, 500), (-22 - 80, 500 + 80), fill = '#5b5b5d')
+ self.draw_rectangle ((-22, -80), (-22 - 80, 0), fill = '#5b5b5d')
+ self.draw_rectangle ((-22, 0), (-22 - 80, 500), fill = '#5b5b5d')
+ self.draw_rectangle ((-22 - 80 - 250, 0), (-22 - 80, 500), fill = '#6d6dad', stipple = 'gray75')
+ self.draw_rectangle ((3000 + 22, 2100), (3000 + 22 + 80, 2100 + 80), fill = '#5b5b5d')
+ self.draw_rectangle ((3000 + 22, 1050 - 40), (3000 + 22 + 80, 1050 + 40), fill = '#5b5b5d')
+ self.draw_rectangle ((3000 + 22, 500), (3000 + 22 + 80, 500 + 80), fill = '#5b5b5d')
+ self.draw_rectangle ((3000 + 22, -80), (3000 + 22 + 80, 0), fill = '#5b5b5d')
+ self.draw_rectangle ((3000 + 22, 0), (3000 + 22 + 80, 500), fill = '#5b5b5d')
+ self.draw_rectangle ((3000 + 22 + 80 + 250, 0), (3000 + 22 + 80, 500), fill = '#6d6dad', stipple = 'gray75')
+ # Vertical dispensers.
+ self.draw_rectangle ((-22, 2100 - 750 - 85 / 2), (0, 2100 - 750 + 85 / 2), fill = '#5b5b5b')
+ self.draw_circle ((40, 2100 - 750), 40)
+ self.draw_rectangle ((700 - 85 / 2, 2100), (700 + 85 / 2, 2100 + 22), fill = '#5b5b5b')
+ self.draw_circle ((700, 2100 - 40), 40)
+ self.draw_rectangle ((3000 + 22, 2100 - 750 - 85 / 2), (3000, 2100 - 750 + 85 / 2), fill = '#5b5b5b')
+ self.draw_circle ((3000 - 40, 2100 - 750), 40)
+ self.draw_rectangle ((3000 - 700 + 85 / 2, 2100), (3000 - 700 - 85 / 2, 2100 + 22), fill = '#5b5b5b')
+ self.draw_circle ((3000 - 700, 2100 - 40), 40)
+ # Horizontal dispenser.
+ self.draw_rectangle ((3000 / 2 - 924 / 2, 2100 + 22), (3000 / 2 + 924 / 2, 2100 + 22 + 80 + 22), fill = '#5b5b5b')
+ self.draw_rectangle ((3000 / 2 - 924 / 2 + 22, 2100 + 22), (3000 / 2 + 924 / 2 - 22, 2100 + 22 + 80), fill = '#5b5b5b')
+ self.draw_rectangle ((3000 / 2 - 880 / 2 - 35 - 60, 2100), (3000 / 2 - 880 / 2 - 35, 2100 + 44), fill = '#5b5b5b')
+ self.draw_rectangle ((3000 / 2 + 880 / 2 + 35 + 60, 2100), (3000 / 2 + 880 / 2 + 35, 2100 + 44), fill = '#5b5b5b')
+ # Balls.
+ balls = [ (800, 200, 'rb'), (800, 400, 'RB'), (800, 600, 'ww'),
+ (1300, 200, 'rb'), (1300, 400, 'rb'), (1300, 600, 'ww'),
+ (520, 800, 'WW'), (700, 40, 'RB'), (40, 750, 'WW'),
+ (450, 1120, 'ww'), (750, 1070, 'ww'), (1050, 1020, 'ww'),
+ (1500 - 72 / 2, -22 - 80 / 2, 'BR'),
+ (1500 - 72 / 2 - 1 * 73, -22 - 80 / 2, 'RB'),
+ (1500 - 72 / 2 - 2 * 73, -22 - 80 / 2, 'BR'),
+ (1500 - 72 / 2 - 3 * 73, -22 - 80 / 2, 'RB'),
+ (1500 - 72 / 2 - 4 * 73, -22 - 80 / 2, 'BR'),
+ (1500 - 72 / 2 - 5 * 73, -22 - 80 / 2, 'RB'),
+ (1500, 1000, 'W'),
+ ]
+ balls_config = { 'r': { 'outline': '#bf4141' }, 'R': { 'fill': '#bf4141' },
+ 'b': { 'outline': '#4241bf' }, 'B': { 'fill': '#4241bf' },
+ 'w': { 'outline': '#bfbfbf' }, 'W': { 'fill': '#bfbfbf' } }
+ for b in balls:
+ self.draw_circle ((3000 - b[0], 2100 - b[1]), 72 / 2,
+ **balls_config[b[2][0]])
+ if len (b[2]) > 1:
+ self.draw_circle ((b[0], 2100 - b[1]), 72 / 2,
+ **balls_config[b[2][1]])
class TableView (DrawableCanvas):
"""This class handle the view of the table and every items inside it."""
@@ -140,102 +140,102 @@ class TableView (DrawableCanvas):
YORIGIN = -2100 / 2
def __init__ (self, master = None):
- DrawableCanvas.__init__ (self, self.WIDTH, self.HEIGHT, self.XORIGIN,
- self.YORIGIN,
- master, borderwidth = 1, relief = 'sunken',
- background = 'white')
- self.table = Table (self)
- self.robot = Robot (self.table)
- self.robot.angle = 0
- self.robot.pos = (0, 0)
- self.drawn = [ ]
+ DrawableCanvas.__init__ (self, self.WIDTH, self.HEIGHT, self.XORIGIN,
+ self.YORIGIN,
+ master, borderwidth = 1, relief = 'sunken',
+ background = 'white')
+ self.table = Table (self)
+ self.robot = Robot (self.table)
+ self.robot.angle = 0
+ self.robot.pos = (0, 0)
+ self.drawn = [ ]
def draw (self):
- self.table.draw ()
- for i in self.drawn:
- i.draw ()
- self.robot.draw ()
+ self.table.draw ()
+ for i in self.drawn:
+ i.draw ()
+ self.robot.draw ()
class Arm (Drawable):
"""The robot arm."""
def draw (self):
- self.reset ()
- self.draw_arc ((0, 0), 0.45, start = 7 * pi / 12, extent = 10 * pi / 12,
- style = 'arc', outline = '#808080')
- self.draw_arc ((0, 0), 0.45, start = -5 * pi / 12, extent = 10 * pi / 12,
- style = 'arc', outline = '#808080')
- self.draw_arc ((0, 0), 0.25, start = -7 * pi / 12, extent = 14 * pi / 12,
- style = 'arc', outline = '#808080')
- self.trans_scale (0.4)
- self.trans_rotate (-self.angle)
- self.draw_line ((0, 0), (0, 1))
- self.draw_line ((0, 1), (0.3, 1), arrow = LAST, fill = '#808080')
- self.draw_line ((0, 0), (cos (pi / 6), -sin (pi / 6)))
- self.draw_line ((0, 0), (-cos (pi / 6), -sin (pi / 6)))
+ self.reset ()
+ self.draw_arc ((0, 0), 0.45, start = 7 * pi / 12, extent = 10 * pi / 12,
+ style = 'arc', outline = '#808080')
+ self.draw_arc ((0, 0), 0.45, start = -5 * pi / 12, extent = 10 * pi / 12,
+ style = 'arc', outline = '#808080')
+ self.draw_arc ((0, 0), 0.25, start = -7 * pi / 12, extent = 14 * pi / 12,
+ style = 'arc', outline = '#808080')
+ self.trans_scale (0.4)
+ self.trans_rotate (-self.angle)
+ self.draw_line ((0, 0), (0, 1))
+ self.draw_line ((0, 1), (0.3, 1), arrow = LAST, fill = '#808080')
+ self.draw_line ((0, 0), (cos (pi / 6), -sin (pi / 6)))
+ self.draw_line ((0, 0), (-cos (pi / 6), -sin (pi / 6)))
class Servo:
"""Servo motor."""
def __init__ (self, coord, l, start, extent):
- self.coord = coord
- self.l = l
- self.start = start
- self.extent = extent
- self.pos = 0
+ self.coord = coord
+ self.l = l
+ self.start = start
+ self.extent = extent
+ self.pos = 0
def draw (self, d):
- d.draw_arc (self.coord, self.l, start = self.start,
- extent = self.extent, style = 'arc', outline = '#808080')
- a = self.start + self.pos * self.extent
- d.draw_line (self.coord, (self.coord[0] + self.l * cos (a),
- self.coord[1] + self.l * sin (a)))
+ d.draw_arc (self.coord, self.l, start = self.start,
+ extent = self.extent, style = 'arc', outline = '#808080')
+ a = self.start + self.pos * self.extent
+ d.draw_line (self.coord, (self.coord[0] + self.l * cos (a),
+ self.coord[1] + self.l * sin (a)))
class Rear (Drawable):
"""Rear actuators."""
def __init__ (self, onto):
- Drawable.__init__ (self, onto)
- self.traps = [
- Servo ((-2.5, -1), 0.8, 0, pi/2),
- Servo ((-1.5, -0.9), 0.8, 0, pi/2),
- Servo ((-0.5, -0.8), 0.8, 0, pi/2),
- Servo ((0.5, -0.8), 0.8, pi, -pi/2),
- Servo ((1.5, -0.9), 0.8, pi, -pi/2),
- Servo ((-2.5, 1.3), 0.8, -pi/6, pi/3),
- ]
+ Drawable.__init__ (self, onto)
+ self.traps = [
+ Servo ((-2.5, -1), 0.8, 0, pi/2),
+ Servo ((-1.5, -0.9), 0.8, 0, pi/2),
+ Servo ((-0.5, -0.8), 0.8, 0, pi/2),
+ Servo ((0.5, -0.8), 0.8, pi, -pi/2),
+ Servo ((1.5, -0.9), 0.8, pi, -pi/2),
+ Servo ((-2.5, 1.3), 0.8, -pi/6, pi/3),
+ ]
def draw (self):
- self.reset ()
- self.trans_scale (0.9/5)
- for i in self.traps:
- i.draw (self)
- self.draw_line ((-0.5, 1.5), (-0.5, 0.5), (-2.5, 0.2),
- fill = '#808080')
- self.draw_line ((-2.5, -1.2), (-2.5, -2.3), (2.5, -2.3), (2.5, 0.2),
- (0.5, 0.5), (0.5, 1.5), fill = '#808080')
- for i in (-1.5, -0.5, 0.5, 1.5):
- self.draw_line ((i, -2.3), (i, -2), fill = '#808080')
+ self.reset ()
+ self.trans_scale (0.9/5)
+ for i in self.traps:
+ i.draw (self)
+ self.draw_line ((-0.5, 1.5), (-0.5, 0.5), (-2.5, 0.2),
+ fill = '#808080')
+ self.draw_line ((-2.5, -1.2), (-2.5, -2.3), (2.5, -2.3), (2.5, 0.2),
+ (0.5, 0.5), (0.5, 1.5), fill = '#808080')
+ for i in (-1.5, -0.5, 0.5, 1.5):
+ self.draw_line ((i, -2.3), (i, -2), fill = '#808080')
class ActuatorView (DrawableCanvas):
"""This class handle the view of the actuators inside the robot."""
def __init__ (self, master = None):
- DrawableCanvas.__init__ (self, 1, 2, 0, 0, master,
- borderwidth = 1, relief = 'sunken', background = 'white')
- self.configure (width = 120, height = 240)
- self.arm_drawable = Drawable (self)
- self.arm_drawable.trans_translate ((0, 0.5))
- self.arm = Arm (self.arm_drawable)
- self.arm.angle = 0
- self.rear_drawable = Drawable (self)
- self.rear_drawable.trans_translate ((0, -0.5))
- self.rear = Rear (self.rear_drawable)
+ DrawableCanvas.__init__ (self, 1, 2, 0, 0, master,
+ borderwidth = 1, relief = 'sunken', background = 'white')
+ self.configure (width = 120, height = 240)
+ self.arm_drawable = Drawable (self)
+ self.arm_drawable.trans_translate ((0, 0.5))
+ self.arm = Arm (self.arm_drawable)
+ self.arm.angle = 0
+ self.rear_drawable = Drawable (self)
+ self.rear_drawable.trans_translate ((0, -0.5))
+ self.rear = Rear (self.rear_drawable)
def draw (self):
- self.arm.draw ()
- self.rear.draw ()
+ self.arm.draw ()
+ self.rear.draw ()
class Inter (Frame):
"""Robot simulation interface."""
@@ -244,54 +244,54 @@ class Inter (Frame):
Frame.__init__ (self, master)
self.pack (expand = 1, fill = 'both')
self.createWidgets ()
- self.updated = [ ]
+ self.updated = [ ]
def createWidgets (self):
- self.rightFrame = Frame (self)
- self.rightFrame.pack (side = 'right', fill = 'y')
- self.quitButton = Button (self.rightFrame, text = 'Quit', command = self.quit)
- self.quitButton.pack (side = 'top')
- self.actuatorview = ActuatorView (self.rightFrame)
- self.actuatorview.pack (side = 'bottom', fill = 'x')
-
- self.sensorFrame = Frame (self.rightFrame, borderwidth = 1, relief =
- 'sunken')
- self.sensorFrame.pack (side = 'bottom', fill = 'x')
- self.jackVar = IntVar ()
- self.jackButton = Checkbutton (self.sensorFrame, text = 'Jack',
- indicatoron = False, variable = self.jackVar)
- self.jackButton.pack (side = 'top')
- self.colorVar = IntVar ()
- self.colorButton = Checkbutton (self.sensorFrame, text = 'Color',
- indicatoron = False, variable = self.colorVar)
- self.colorButton.pack (side = 'top')
-
- self.tableview = TableView (self)
- self.tableview.pack (expand = True, fill = 'both')
+ self.rightFrame = Frame (self)
+ self.rightFrame.pack (side = 'right', fill = 'y')
+ self.quitButton = Button (self.rightFrame, text = 'Quit', command = self.quit)
+ self.quitButton.pack (side = 'top')
+ self.actuatorview = ActuatorView (self.rightFrame)
+ self.actuatorview.pack (side = 'bottom', fill = 'x')
+
+ self.sensorFrame = Frame (self.rightFrame, borderwidth = 1, relief =
+ 'sunken')
+ self.sensorFrame.pack (side = 'bottom', fill = 'x')
+ self.jackVar = IntVar ()
+ self.jackButton = Checkbutton (self.sensorFrame, text = 'Jack',
+ indicatoron = False, variable = self.jackVar)
+ self.jackButton.pack (side = 'top')
+ self.colorVar = IntVar ()
+ self.colorButton = Checkbutton (self.sensorFrame, text = 'Color',
+ indicatoron = False, variable = self.colorVar)
+ self.colorButton.pack (side = 'top')
+
+ self.tableview = TableView (self)
+ self.tableview.pack (expand = True, fill = 'both')
def update (self, *args):
- """If called with arguments, add them to the list of objects to be
- updated.
- If called without argument, redraw all objects to be updated."""
- if args:
- for i in args:
- if i not in self.updated:
- self.updated.append (i)
- else:
- for i in self.updated:
- i.draw ()
- self.updated = [ ]
+ """If called with arguments, add them to the list of objects to be
+ updated.
+ If called without argument, redraw all objects to be updated."""
+ if args:
+ for i in args:
+ if i not in self.updated:
+ self.updated.append (i)
+ else:
+ for i in self.updated:
+ i.draw ()
+ self.updated = [ ]
if __name__ == '__main__':
app = Inter ()
app.tableview.robot.angle = pi / 3
app.tableview.robot.pos = (700, 700)
if 0:
- from dist_sensor import DistSensor
- ds = DistSensor (app.tableview.robot, (150, -127), -pi / 12, 800)
- app.tableview.robot.drawn.append (ds)
- app.tableview.drawn.append (Obstacle (app.tableview, (1300, 1200), 150))
- ds.obstacles = app.tableview.drawn
+ from dist_sensor import DistSensor
+ ds = DistSensor (app.tableview.robot, (150, -127), -pi / 12, 800)
+ app.tableview.robot.drawn.append (ds)
+ app.tableview.drawn.append (Obstacle (app.tableview, (1300, 1200), 150))
+ ds.obstacles = app.tableview.drawn
app.actuatorview.arm.angle = pi/6
app.actuatorview.rear.traps[0].pos = 1
app.actuatorview.rear.traps[1].pos = 0
diff --git a/host/inter/inter_node.py b/host/inter/inter_node.py
index 96648c6f..12aaee6e 100644
--- a/host/inter/inter_node.py
+++ b/host/inter/inter_node.py
@@ -49,160 +49,160 @@ class InterNode (Inter):
IO_PATH = 0xb4
def __init__ (self):
- Inter.__init__ (self)
- self.node = Node ()
- self.node.register (0xa0, self.handle_asserv_0)
- self.node.register (0xa8, self.handle_asserv_8)
- self.node.register (self.IO_JACK, self.handle_IO_JACK)
- self.node.register (self.IO_COLOR, self.handle_IO_COLOR)
- self.node.register (self.IO_SERVO, self.handle_IO_SERVO)
- self.node.register (self.IO_SHARPS, self.handle_IO_SHARPS)
- self.node.register (self.IO_PATH, self.handle_IO_PATH)
- self.tk.createfilehandler (self.node, READABLE, self.read)
- self.date = 0
- self.synced = True
- self.step_after = None
- self.step_time = None
- self.obstacles = [ ]
- self.dist_sensors = [
- DistSensor (self.tableview.robot, (150, 127), 0, 800),
- DistSensor (self.tableview.robot, (150, 0), 0, 800),
- DistSensor (self.tableview.robot, (150, -127), 0, 800),
- DistSensor (self.tableview.robot, (-70, 100), pi, 800),
- DistSensor (self.tableview.robot, (-70, -100), pi, 800),
- ]
- for s in self.dist_sensors:
- s.obstacles = self.obstacles
- s.hide = True
- self.tableview.robot.drawn.extend (self.dist_sensors)
- self.path = Path (self.tableview.table)
- self.tableview.drawn.append (self.path)
- self.tableview
+ Inter.__init__ (self)
+ self.node = Node ()
+ self.node.register (0xa0, self.handle_asserv_0)
+ self.node.register (0xa8, self.handle_asserv_8)
+ self.node.register (self.IO_JACK, self.handle_IO_JACK)
+ self.node.register (self.IO_COLOR, self.handle_IO_COLOR)
+ self.node.register (self.IO_SERVO, self.handle_IO_SERVO)
+ self.node.register (self.IO_SHARPS, self.handle_IO_SHARPS)
+ self.node.register (self.IO_PATH, self.handle_IO_PATH)
+ self.tk.createfilehandler (self.node, READABLE, self.read)
+ self.date = 0
+ self.synced = True
+ self.step_after = None
+ self.step_time = None
+ self.obstacles = [ ]
+ self.dist_sensors = [
+ DistSensor (self.tableview.robot, (150, 127), 0, 800),
+ DistSensor (self.tableview.robot, (150, 0), 0, 800),
+ DistSensor (self.tableview.robot, (150, -127), 0, 800),
+ DistSensor (self.tableview.robot, (-70, 100), pi, 800),
+ DistSensor (self.tableview.robot, (-70, -100), pi, 800),
+ ]
+ for s in self.dist_sensors:
+ s.obstacles = self.obstacles
+ s.hide = True
+ self.tableview.robot.drawn.extend (self.dist_sensors)
+ self.path = Path (self.tableview.table)
+ self.tableview.drawn.append (self.path)
+ self.tableview
def createWidgets (self):
- Inter.createWidgets (self)
- self.nowLabel = Label (self.rightFrame, text = 'Now: 0 s')
- self.nowLabel.pack ()
- self.stepButton = Button (self.rightFrame, text = 'Step',
- command = self.step)
- self.stepButton.pack ()
- self.stepSizeScale = Scale (self.rightFrame, orient = HORIZONTAL,
- from_ = 0.05, to = 1.0, resolution = 0.05)
- self.stepSizeScale.pack ()
- self.playVar = IntVar ()
- self.playButton = Checkbutton (self.rightFrame, variable =
- self.playVar, text = 'Play', command = self.play)
- self.playButton.pack ()
- self.tableview.bind ('<2>', self.place_obstacle)
- self.showSensorsVar = IntVar ()
- self.showSensorsButton = Checkbutton (self.sensorFrame, variable =
- self.showSensorsVar, text = 'Show sensors', command =
- self.show_sensors)
- self.showSensorsButton.pack ()
+ Inter.createWidgets (self)
+ self.nowLabel = Label (self.rightFrame, text = 'Now: 0 s')
+ self.nowLabel.pack ()
+ self.stepButton = Button (self.rightFrame, text = 'Step',
+ command = self.step)
+ self.stepButton.pack ()
+ self.stepSizeScale = Scale (self.rightFrame, orient = HORIZONTAL,
+ from_ = 0.05, to = 1.0, resolution = 0.05)
+ self.stepSizeScale.pack ()
+ self.playVar = IntVar ()
+ self.playButton = Checkbutton (self.rightFrame, variable =
+ self.playVar, text = 'Play', command = self.play)
+ self.playButton.pack ()
+ self.tableview.bind ('<2>', self.place_obstacle)
+ self.showSensorsVar = IntVar ()
+ self.showSensorsButton = Checkbutton (self.sensorFrame, variable =
+ self.showSensorsVar, text = 'Show sensors', command =
+ self.show_sensors)
+ self.showSensorsButton.pack ()
def step (self):
- """Do a step. Signal to the Hub we are ready to wait to the next step
- date."""
- self.node.wait_async (self.date
- + int (self.stepSizeScale.get () * self.TICK))
- self.synced = False
- self.step_after = None
- self.step_time = time.time ()
+ """Do a step. Signal to the Hub we are ready to wait to the next step
+ date."""
+ self.node.wait_async (self.date
+ + int (self.stepSizeScale.get () * self.TICK))
+ self.synced = False
+ self.step_after = None
+ self.step_time = time.time ()
def play (self):
- """Activate auto-steping."""
- if self.playVar.get ():
- if self.step_after is None and self.synced:
- self.step ()
- self.stepButton.configure (state = DISABLED)
- else:
- if self.step_after is not None:
- self.after_cancel (self.step_after)
- self.step_after = None
- self.stepButton.configure (state = NORMAL)
+ """Activate auto-steping."""
+ if self.playVar.get ():
+ if self.step_after is None and self.synced:
+ self.step ()
+ self.stepButton.configure (state = DISABLED)
+ else:
+ if self.step_after is not None:
+ self.after_cancel (self.step_after)
+ self.step_after = None
+ self.stepButton.configure (state = NORMAL)
def read (self, file, mask):
- """Handle event on the Node."""
- self.node.read ()
- if not self.synced and self.node.sync ():
- self.synced = True
- self.date = self.node.date
- self.nowLabel.configure (text = 'Now: %.2f s' % (self.date
- / self.TICK))
- self.update ()
- if self.playVar.get ():
- assert self.step_after is None
- next = self.step_time + self.stepSizeScale.get ()
- delay = next - time.time ()
- if delay > 0:
- self.step_after = self.after (int (delay * 1000),
- self.step)
- else:
- self.step ()
+ """Handle event on the Node."""
+ self.node.read ()
+ if not self.synced and self.node.sync ():
+ self.synced = True
+ self.date = self.node.date
+ self.nowLabel.configure (text = 'Now: %.2f s' % (self.date
+ / self.TICK))
+ self.update ()
+ if self.playVar.get ():
+ assert self.step_after is None
+ next = self.step_time + self.stepSizeScale.get ()
+ delay = next - time.time ()
+ if delay > 0:
+ self.step_after = self.after (int (delay * 1000),
+ self.step)
+ else:
+ self.step ()
def handle_asserv_0 (self, msg):
- x, y, a = msg.pop ('hhl')
- self.tableview.robot.pos = (x, y)
- self.tableview.robot.angle = float (a) / 1024
- self.update (self.tableview.robot)
+ x, y, a = msg.pop ('hhl')
+ self.tableview.robot.pos = (x, y)
+ self.tableview.robot.angle = float (a) / 1024
+ self.update (self.tableview.robot)
def handle_asserv_8 (self, msg):
- a, = msg.pop ('l')
- self.actuatorview.arm.angle = float (a) / 1024
- self.update (self.actuatorview.arm)
+ a, = msg.pop ('l')
+ self.actuatorview.arm.angle = float (a) / 1024
+ self.update (self.actuatorview.arm)
def handle_IO_JACK (self, msg):
- m = Msg (self.IO_JACK)
- m.push ('B', self.jackVar.get ())
- self.node.response (m)
+ m = Msg (self.IO_JACK)
+ m.push ('B', self.jackVar.get ())
+ self.node.response (m)
def handle_IO_COLOR (self, msg):
- m = Msg (self.IO_COLOR)
- m.push ('B', self.colorVar.get ())
- self.node.response (m)
+ m = Msg (self.IO_COLOR)
+ m.push ('B', self.colorVar.get ())
+ self.node.response (m)
def handle_IO_SERVO (self, msg):
- for t in self.actuatorview.rear.traps:
- t.pos = float (msg.pop ('B')[0]) / 255
- self.update (self.actuatorview.rear)
+ for t in self.actuatorview.rear.traps:
+ t.pos = float (msg.pop ('B')[0]) / 255
+ self.update (self.actuatorview.rear)
def handle_IO_SHARPS (self, msg):
- m = Msg (self.IO_SHARPS)
- for i in self.dist_sensors:
- d = i.distance or 800
- d /= 10
- if d > 10:
- v = 0.000571429 * d*d + -0.0752381 * d + 2.89107
- else:
- v = 2.2 / 10 * d
- v *= 1024 / 5
- m.push ('H', v)
- assert v >= 0 and v < 1024
- self.node.response (m)
+ m = Msg (self.IO_SHARPS)
+ for i in self.dist_sensors:
+ d = i.distance or 800
+ d /= 10
+ if d > 10:
+ v = 0.000571429 * d*d + -0.0752381 * d + 2.89107
+ else:
+ v = 2.2 / 10 * d
+ v *= 1024 / 5
+ m.push ('H', v)
+ assert v >= 0 and v < 1024
+ self.node.response (m)
def handle_IO_PATH (self, msg):
- self.path.path = [ ]
- while len (msg) >= 4:
- self.path.path.append (msg.pop ('hh'))
- self.update (self.path)
+ self.path.path = [ ]
+ while len (msg) >= 4:
+ self.path.path.append (msg.pop ('hh'))
+ self.update (self.path)
def place_obstacle (self, ev):
- pos = self.tableview.screen_coord ((ev.x, ev.y))
- if self.obstacles:
- self.obstacles[0].pos = pos
- else:
- self.obstacles.append (Obstacle (self.tableview.table, pos, 150))
- self.tableview.drawn.append (self.obstacles[0])
- self.update (*self.obstacles)
- self.update (*self.dist_sensors)
- self.update ()
+ pos = self.tableview.screen_coord ((ev.x, ev.y))
+ if self.obstacles:
+ self.obstacles[0].pos = pos
+ else:
+ self.obstacles.append (Obstacle (self.tableview.table, pos, 150))
+ self.tableview.drawn.append (self.obstacles[0])
+ self.update (*self.obstacles)
+ self.update (*self.dist_sensors)
+ self.update ()
def show_sensors (self):
- hide = not self.showSensorsVar.get ()
- for i in self.dist_sensors:
- i.hide = hide
- self.update (*self.dist_sensors)
- self.update ()
+ hide = not self.showSensorsVar.get ()
+ for i in self.dist_sensors:
+ i.hide = hide
+ self.update (*self.dist_sensors)
+ self.update ()
if __name__ == '__main__':
import mex.hub
@@ -210,9 +210,9 @@ if __name__ == '__main__':
h = mex.hub.Hub (min_clients = 1)
fh = utils.forked.Forked (h.wait)
try:
- app = InterNode ()
- app.mainloop()
+ app = InterNode ()
+ app.mainloop()
finally:
- fh.kill ()
- import time
- time.sleep (1)
+ fh.kill ()
+ import time
+ time.sleep (1)
diff --git a/host/inter/path.py b/host/inter/path.py
index 76db970a..eb5744eb 100644
--- a/host/inter/path.py
+++ b/host/inter/path.py
@@ -32,11 +32,11 @@ class Path (Drawable):
"""Computed path drawing."""
def __init__ (self, onto):
- Drawable.__init__ (self, onto)
- self.path = [ ]
+ Drawable.__init__ (self, onto)
+ self.path = [ ]
def draw (self):
- self.reset ()
- if len (self.path) > 2:
- fmt = dict (fill = 'green', arrow = LAST)
- self.draw_line (*self.path, **fmt)
+ self.reset ()
+ if len (self.path) > 2:
+ fmt = dict (fill = 'green', arrow = LAST)
+ self.draw_line (*self.path, **fmt)
diff --git a/host/inter/test/test_drawable.py b/host/inter/test/test_drawable.py
index 79948e12..6f86dd2d 100644
--- a/host/inter/test/test_drawable.py
+++ b/host/inter/test/test_drawable.py
@@ -7,38 +7,38 @@ from math import pi
class Test (Drawable):
def draw (self):
- self.draw_rectangle ((-100, -100), (100, 100), fill = '', outline = 'gray')
- self.draw_rectangle ((0, 0), (5, 5), fill = 'red')
- self.draw_rectangle ((20, 20), (50, 50), fill = '', outline = 'blue')
- self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), fill = 'green')
- self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), smooth = True)
- self.draw_circle ((40, -40), 10)
- self.draw_arc ((-40, 0), 20, start = pi / 4, extent = pi / 2)
+ self.draw_rectangle ((-100, -100), (100, 100), fill = '', outline = 'gray')
+ self.draw_rectangle ((0, 0), (5, 5), fill = 'red')
+ self.draw_rectangle ((20, 20), (50, 50), fill = '', outline = 'blue')
+ self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), fill = 'green')
+ self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), smooth = True)
+ self.draw_circle ((40, -40), 10)
+ self.draw_arc ((-40, 0), 20, start = pi / 4, extent = pi / 2)
class App (DrawableCanvas):
def __init__ (self, master = None):
- DrawableCanvas.__init__ (self, 300, 300, 20, 20, master)
- self.pack (expand = True, fill = 'both')
- self.test = Test (self)
- self.animated = False
- self.i = 0
+ DrawableCanvas.__init__ (self, 300, 300, 20, 20, master)
+ self.pack (expand = True, fill = 'both')
+ self.test = Test (self)
+ self.animated = False
+ self.i = 0
def animate (self):
- # Real user should reset at each redraw.
- self.after (500, self.animate)
- self.test.draw ()
- self.test.trans_rotate (-pi/12)
- self.test.trans_translate ((10, 10))
- self.test.trans_scale (1.05)
- self.i += 1
- if self.i == 10:
- self.test.reset ()
+ # Real user should reset at each redraw.
+ self.after (500, self.animate)
+ self.test.draw ()
+ self.test.trans_rotate (-pi/12)
+ self.test.trans_translate ((10, 10))
+ self.test.trans_scale (1.05)
+ self.i += 1
+ if self.i == 10:
+ self.test.reset ()
def draw (self):
- if not self.animated:
- self.animate ()
- self.animated = True
+ if not self.animated:
+ self.animate ()
+ self.animated = True
app = App ()
app.mainloop ()
diff --git a/host/inter/trans_matrix.py b/host/inter/trans_matrix.py
index dcaab134..a8f3849a 100644
--- a/host/inter/trans_matrix.py
+++ b/host/inter/trans_matrix.py
@@ -33,138 +33,138 @@ class TransMatrix:
IDENTITY = ((1, 0), (0, 1), (0, 0))
def __init__ (self, *m):
- """Initialise the matrix, with optional initial value.
-
- >>> TransMatrix ()
- ((1, 0), (0, 1), (0, 0))
- >>> TransMatrix ((1, 2), (3, 4), (5, 6))
- ((1, 2), (3, 4), (5, 6))
- """
- if m:
- assert len (m) == 3
- for i in m:
- assert len (i) == 2
- self.matrix = m
- else:
- self.matrix = self.IDENTITY
+ """Initialise the matrix, with optional initial value.
+
+ >>> TransMatrix ()
+ ((1, 0), (0, 1), (0, 0))
+ >>> TransMatrix ((1, 2), (3, 4), (5, 6))
+ ((1, 2), (3, 4), (5, 6))
+ """
+ if m:
+ assert len (m) == 3
+ for i in m:
+ assert len (i) == 2
+ self.matrix = m
+ else:
+ self.matrix = self.IDENTITY
def identity (self):
- """Set to identity.
+ """Set to identity.
- >>> a = TransMatrix ()
- >>> a.translate ((2, 3))
- >>> a.identity (); a
- ((1, 0), (0, 1), (0, 0))
- """
- self.matrix = self.IDENTITY
+ >>> a = TransMatrix ()
+ >>> a.translate ((2, 3))
+ >>> a.identity (); a
+ ((1, 0), (0, 1), (0, 0))
+ """
+ self.matrix = self.IDENTITY
def rotate (self, angle):
- """Transform the current matrix to do a rotation.
-
- >>> from math import pi
- >>> a = TransMatrix ()
- >>> a.rotate (pi / 3); a # doctest: +ELLIPSIS
- ((0.5..., 0.866...), (-0.866..., 0.5...), (0.0, 0.0))
- """
- s = sin (angle)
- c = cos (angle)
- m = TransMatrix ((c, s), (-s, c), (0, 0))
- self *= m
+ """Transform the current matrix to do a rotation.
+
+ >>> from math import pi
+ >>> a = TransMatrix ()
+ >>> a.rotate (pi / 3); a # doctest: +ELLIPSIS
+ ((0.5..., 0.866...), (-0.866..., 0.5...), (0.0, 0.0))
+ """
+ s = sin (angle)
+ c = cos (angle)
+ m = TransMatrix ((c, s), (-s, c), (0, 0))
+ self *= m
def translate (self, by):
- """Transform the current matrix to do a translation.
-
- >>> a = TransMatrix ()
- >>> a.translate ((2, 3)); a
- ((1, 0), (0, 1), (2, 3))
- """
- m = TransMatrix ((1, 0), (0, 1), by)
- self *= m
+ """Transform the current matrix to do a translation.
+
+ >>> a = TransMatrix ()
+ >>> a.translate ((2, 3)); a
+ ((1, 0), (0, 1), (2, 3))
+ """
+ m = TransMatrix ((1, 0), (0, 1), by)
+ self *= m
def scale (self, factor):
- """Transform the current matrix to do a scaling.
-
- >>> a = TransMatrix ()
- >>> a.scale (2); a
- ((2, 0), (0, 2), (0, 0))
- """
- m = TransMatrix ((factor, 0), (0, factor), (0, 0))
- self *= m
+ """Transform the current matrix to do a scaling.
+
+ >>> a = TransMatrix ()
+ >>> a.scale (2); a
+ ((2, 0), (0, 2), (0, 0))
+ """
+ m = TransMatrix ((factor, 0), (0, factor), (0, 0))
+ self *= m
def __imul__ (self, other):
- """Multiply by an other matrix.
-
- >>> a = TransMatrix ((1, 0), (0, 1), (1, 0))
- >>> b = TransMatrix ((0, 1), (1, 0), (0, 1))
- >>> a *= b; a
- ((0, 1), (1, 0), (0, 2))
- """
- s = self.matrix
- o = other.matrix
- self.matrix = (
- (s[0][0] * o[0][0] + s[0][1] * o[1][0],
- s[0][0] * o[0][1] + s[0][1] * o[1][1]),
- (s[1][0] * o[0][0] + s[1][1] * o[1][0],
- s[1][0] * o[0][1] + s[1][1] * o[1][1]),
- (s[2][0] * o[0][0] + s[2][1] * o[1][0] + o[2][0],
- s[2][0] * o[0][1] + s[2][1] * o[1][1] + o[2][1]))
- return self
+ """Multiply by an other matrix.
+
+ >>> a = TransMatrix ((1, 0), (0, 1), (1, 0))
+ >>> b = TransMatrix ((0, 1), (1, 0), (0, 1))
+ >>> a *= b; a
+ ((0, 1), (1, 0), (0, 2))
+ """
+ s = self.matrix
+ o = other.matrix
+ self.matrix = (
+ (s[0][0] * o[0][0] + s[0][1] * o[1][0],
+ s[0][0] * o[0][1] + s[0][1] * o[1][1]),
+ (s[1][0] * o[0][0] + s[1][1] * o[1][0],
+ s[1][0] * o[0][1] + s[1][1] * o[1][1]),
+ (s[2][0] * o[0][0] + s[2][1] * o[1][0] + o[2][0],
+ s[2][0] * o[0][1] + s[2][1] * o[1][1] + o[2][1]))
+ return self
def apply (self, *args):
- """Apply (multiply) the matrix to all the given arguments.
-
- >>> m = TransMatrix ((1, 2), (4, 8), (16, 32))
- >>> m.apply ((1, 0))
- (17, 34)
- >>> m.apply ((0, 1), (1, 1))
- ((20, 40), (21, 42))
- """
- r = tuple (
- (i[0] * self.matrix[0][0] + i[1] * self.matrix[1][0]
- + self.matrix[2][0],
- i[0] * self.matrix[0][1] + i[1] * self.matrix[1][1]
- + self.matrix[2][1])
- for i in args)
- if len (args) == 1:
- return r[0]
- else:
- return r
+ """Apply (multiply) the matrix to all the given arguments.
+
+ >>> m = TransMatrix ((1, 2), (4, 8), (16, 32))
+ >>> m.apply ((1, 0))
+ (17, 34)
+ >>> m.apply ((0, 1), (1, 1))
+ ((20, 40), (21, 42))
+ """
+ r = tuple (
+ (i[0] * self.matrix[0][0] + i[1] * self.matrix[1][0]
+ + self.matrix[2][0],
+ i[0] * self.matrix[0][1] + i[1] * self.matrix[1][1]
+ + self.matrix[2][1])
+ for i in args)
+ if len (args) == 1:
+ return r[0]
+ else:
+ return r
def apply_angle (self, angle):
- """Apply the matrix to an angle.
-
- >>> from math import pi
- >>> a = TransMatrix ()
- >>> a.rotate (pi / 6)
- >>> a.translate ((2, 3))
- >>> a.scale (4)
- >>> a.apply_angle (pi / 6), pi / 3 # doctest: +ELLIPSIS
- (1.0471..., 1.0471...)
- """
- o, m = self.apply ((0, 0), (cos (angle), sin (angle)))
- v = (m[0] - o[0], m[1] - o[1])
- vl = sqrt (v[0] ** 2 + v[1] ** 2)
- v = (v[0] / vl, v[1] / vl)
- return atan2 (v[1], v[0])
+ """Apply the matrix to an angle.
+
+ >>> from math import pi
+ >>> a = TransMatrix ()
+ >>> a.rotate (pi / 6)
+ >>> a.translate ((2, 3))
+ >>> a.scale (4)
+ >>> a.apply_angle (pi / 6), pi / 3 # doctest: +ELLIPSIS
+ (1.0471..., 1.0471...)
+ """
+ o, m = self.apply ((0, 0), (cos (angle), sin (angle)))
+ v = (m[0] - o[0], m[1] - o[1])
+ vl = sqrt (v[0] ** 2 + v[1] ** 2)
+ v = (v[0] / vl, v[1] / vl)
+ return atan2 (v[1], v[0])
def apply_distance (self, distance):
- """Apply the matrix to a distance.
-
- >>> from math import pi
- >>> a = TransMatrix ()
- >>> a.rotate (pi / 6)
- >>> a.translate ((2, 3))
- >>> a.scale (4)
- >>> round (a.apply_distance (2))
- 8.0
- """
- o, m = self.apply ((0, 0), (distance, 0))
- v = (m[0] - o[0], m[1] - o[1])
- vl = sqrt (v[0] ** 2 + v[1] ** 2)
- return vl
+ """Apply the matrix to a distance.
+
+ >>> from math import pi
+ >>> a = TransMatrix ()
+ >>> a.rotate (pi / 6)
+ >>> a.translate ((2, 3))
+ >>> a.scale (4)
+ >>> round (a.apply_distance (2))
+ 8.0
+ """
+ o, m = self.apply ((0, 0), (distance, 0))
+ v = (m[0] - o[0], m[1] - o[1])
+ vl = sqrt (v[0] ** 2 + v[1] ** 2)
+ return vl
def __repr__ (self):
- return self.matrix.__repr__ ()
+ return self.matrix.__repr__ ()
def _test ():
import doctest