From 201aa07cbd28c6be8c4003f126b1ae258f859c68 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 22:44:20 +0200 Subject: host/simu: handle dropping tower on the table --- host/simu/model/table.py | 4 +- host/simu/model/table_eurobot2011.py | 6 +++ host/simu/robots/robospierre/model/clamp.py | 64 ++++++++++++++++++++++++----- host/simu/view/table_eurobot2011.py | 9 +++- 4 files changed, 70 insertions(+), 13 deletions(-) (limited to 'host') 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.""" -- cgit v1.2.3