From 73f25d3310706c5d7a559f120805c54860b16795 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 20:14:12 +0200 Subject: host/simu/robots/robospierre: add tower management --- host/simu/robots/robospierre/model/clamp.py | 60 ++++++++++++++++++++++++----- 1 file changed, 51 insertions(+), 9 deletions(-) (limited to 'host/simu/robots/robospierre/model') diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index e2592aa8..976e559f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -24,6 +24,7 @@ """Robospierre clamp.""" from utils.observable import Observable from simu.utils.trans_matrix import TransMatrix +from simu.model.round_obstacle import RoundObstacle from math import pi, cos, sin class Slot: @@ -36,12 +37,7 @@ class Slot: self.side = side self.door_motor = door_motor self.contact = contact - self.set_pawn (None) - - def set_pawn (self, pawn): - self.pawn = pawn - self.contact.state = pawn is None - self.contact.notify () + self.pawn = None class Clamp (Observable): @@ -90,6 +86,14 @@ class Clamp (Observable): door_motors[3], slot_contacts[5]), Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, None, slot_contacts[6])) + self.front_slots = ( + self.slots[self.SLOT_FRONT_BOTTOM], + self.slots[self.SLOT_FRONT_MIDDLE], + self.slots[self.SLOT_FRONT_TOP]) + self.back_slots = ( + self.slots[self.SLOT_BACK_BOTTOM], + self.slots[self.SLOT_BACK_MIDDLE], + self.slots[self.SLOT_BACK_TOP]) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) @@ -104,11 +108,12 @@ class Clamp (Observable): if slot.pawn is None: p = self.__get_floor_elements (slot.side) if p is not None: - slot.set_pawn (p) + slot.pawn = p p.pos = None p.notify () changed = True if changed: + self.update_contacts () self.notify () def __elevation_notified (self): @@ -141,16 +146,53 @@ class Clamp (Observable): slot = self.__get_clamp_slot () if slot and slot.pawn is not None: self.load = slot.pawn - slot.set_pawn (None) + slot.pawn = None elif self.clamping == self.CLAMPING_STROKE \ and self.load is not None: # Unload an element. slot = self.__get_clamp_slot () if slot and slot.pawn is None: - slot.set_pawn (self.load) + slot.pawn = self.load self.load = None + # Little resources saving hack: done here, all motors are notified + # at the same time. + self.check_tower () + self.update_contacts () self.notify () + def check_tower (self): + """Check whether several elements can make a tower.""" + for slots in (self.front_slots, self.back_slots): + if slots[0].pawn is not None and slots[1].pawn is not None: + assert slots[0].pawn.kind != 'tower' + tower = RoundObstacle (100, 1) + tower.kind = 'tower' + tower.tower = [ slots[0].pawn, slots[1].pawn ] + slots[0].pawn, slots[1].pawn = tower, None + 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) + slots[2].pawn = None + + def update_contacts (self): + """Update pawn contacts.""" + for slots in (self.front_slots, self.back_slots): + slots[0].contact.state = not (slots[0].pawn is not None) + # A tower at level 0 is seen at level 1. + slots[1].contact.state = not ( + slots[1].pawn is not None + or (slots[0].pawn is not None + and slots[0].pawn.kind == 'tower')) + # This one is really high. + slots[2].contact.state = not (slots[2].pawn is not None) + slot_side = self.slots[self.SLOT_SIDE] + slot_side.contact.state = slot_side.pawn is None + clamp_slot = self.__get_clamp_slot () + if clamp_slot is not None: + clamp_slot.contact.state = False + for slot in self.slots: + slot.contact.notify () + def __get_floor_elements (self, side): """Return an elements in front (side = 0) or in back (side = 1) of the robot, on the floor.""" -- cgit v1.2.3