summaryrefslogtreecommitdiff
path: root/host
diff options
context:
space:
mode:
Diffstat (limited to 'host')
-rw-r--r--host/simu/model/table.py4
-rw-r--r--host/simu/model/table_eurobot2011.py6
-rw-r--r--host/simu/robots/robospierre/model/clamp.py64
-rw-r--r--host/simu/view/table_eurobot2011.py9
4 files changed, 70 insertions, 13 deletions
diff --git a/host/simu/model/table.py b/host/simu/model/table.py
index 41767ac4..d79f7758 100644
--- a/host/simu/model/table.py
+++ b/host/simu/model/table.py
@@ -22,6 +22,7 @@
#
# }}}
"""Table model."""
+from utils.observable import Observable
class Intersect:
@@ -29,9 +30,10 @@ class Intersect:
self.obstacle = obstacle
self.distance = distance
-class Table:
+class Table (Observable):
def __init__ (self):
+ Observable.__init__ (self)
self.obstacles = [ ]
def intersect (self, a, b, level = None, comp = None):
diff --git a/host/simu/model/table_eurobot2011.py b/host/simu/model/table_eurobot2011.py
index c9c1d193..f4bb63b7 100644
--- a/host/simu/model/table_eurobot2011.py
+++ b/host/simu/model/table_eurobot2011.py
@@ -81,3 +81,9 @@ class Table (simu.model.table.Table):
self.pawns.append (pawn)
# Add everything to obstacles.
self.obstacles += self.pawns
+
+ def add_pawn (self, pawn):
+ self.pawns.append (pawn)
+ self.obstacles.append (pawn)
+ self.notify ()
+
diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py
index 976e559f..fd3dde7c 100644
--- a/host/simu/robots/robospierre/model/clamp.py
+++ b/host/simu/robots/robospierre/model/clamp.py
@@ -101,16 +101,31 @@ class Clamp (Observable):
self.clamping_motor.register (self.__clamping_notified)
def __robot_position_notified (self):
+ # Compute robot direction.
+ direction = self.__get_robot_direction ()
# Update bottom slots.
changed = False
for sloti in (self.SLOT_FRONT_BOTTOM, self.SLOT_BACK_BOTTOM):
slot = self.slots[sloti]
- if slot.pawn is None:
- p = self.__get_floor_elements (slot.side)
- if p is not None:
- slot.pawn = p
- p.pos = None
- p.notify ()
+ if direction == slot.side or direction is None:
+ # If pushing, can take new elements.
+ if slot.pawn is None:
+ p = self.__get_floor_elements (slot.side)
+ if p is not None:
+ slot.pawn = p
+ p.pos = None
+ p.notify ()
+ changed = True
+ else:
+ # Else, can drop elements.
+ if slot.pawn is not None and slot.door_motor.angle:
+ m = TransMatrix ()
+ m.translate (self.robot_position.pos)
+ m.rotate (self.robot_position.angle)
+ xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[slot.side]
+ slot.pawn.pos = m.apply ((xoffset, 0))
+ slot.pawn.notify ()
+ slot.pawn = None
changed = True
if changed:
self.update_contacts ()
@@ -169,6 +184,7 @@ class Clamp (Observable):
tower.kind = 'tower'
tower.tower = [ slots[0].pawn, slots[1].pawn ]
slots[0].pawn, slots[1].pawn = tower, None
+ self.table.add_pawn (tower)
if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \
and slots[2].pawn and slots[2].door_motor.angle:
slots[0].pawn.tower.append (slots[2].pawn)
@@ -199,10 +215,7 @@ class Clamp (Observable):
if self.robot_position.pos is None:
return None
# Matrix to transform an obstacle position into robot coordinates.
- m = TransMatrix ()
- m.rotate (-self.robot_position.angle)
- m.translate ((-self.robot_position.pos[0],
- -self.robot_position.pos[1]))
+ m = self.__get_robot_matrix ()
# Look up elements.
xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side]
xmargin = 20
@@ -230,3 +243,34 @@ class Clamp (Observable):
return slot
return None
+ def __get_robot_direction (self):
+ """Is robot going forward (0), backward (1), or something else
+ (None)?"""
+ if self.robot_position.pos is None:
+ return None
+ filt = 5
+ if hasattr (self, 'old_robot_position'):
+ m = self.__get_robot_matrix ()
+ oldrel = m.apply (self.old_robot_position)
+ if oldrel[0] < 0 and self.direction_counter < filt:
+ self.direction_counter += 1
+ elif oldrel[0] > 0 and self.direction_counter > -filt:
+ self.direction_counter -= 1
+ else:
+ self.direction_counter = 0
+ self.old_robot_position = self.robot_position.pos
+ # Filter oscillations.
+ if self.direction_counter > 0:
+ return 0
+ elif self.direction_counter < 0:
+ return 1
+ else:
+ return None
+
+ def __get_robot_matrix (self):
+ """Return robot transformation matrix."""
+ m = TransMatrix ()
+ m.rotate (-self.robot_position.angle)
+ m.translate ((-self.robot_position.pos[0],
+ -self.robot_position.pos[1]))
+ return m
diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py
index 5725e5b8..b39f2834 100644
--- a/host/simu/view/table_eurobot2011.py
+++ b/host/simu/view/table_eurobot2011.py
@@ -76,8 +76,13 @@ class Table (Drawable):
def __init__ (self, onto, model):
Drawable.__init__ (self, onto)
self.model = model
- for e in model.pawns:
- Pawn (self, e)
+ self.model.register (self.__notified)
+ self.__notified ()
+
+ def __notified (self):
+ for e in self.model.pawns:
+ if e not in self.children:
+ Pawn (self, e)
def draw_both (self, primitive, *args, **kargs):
"""Draw a primitive on both sides."""