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 ++++++++++++++++++++++++----- host/simu/robots/robospierre/view/clamp.py | 31 +++++++++------ host/simu/robots/robospierre/view/robot.py | 4 +- 3 files changed, 72 insertions(+), 23 deletions(-) (limited to 'host/simu/robots/robospierre') 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.""" diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py index 130c3eea..07a10929 100644 --- a/host/simu/robots/robospierre/view/clamp.py +++ b/host/simu/robots/robospierre/view/clamp.py @@ -68,7 +68,7 @@ class ClampTop (Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.model.rotation is not None: @@ -82,7 +82,7 @@ class ClampTop (Drawable): if load is not None: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (-self.model.clamping / 43) @@ -107,16 +107,23 @@ class ClampSide (Drawable): if pawn is not None: self.trans_push () self.trans_translate (pos) - self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) - if pawn.kind == 'king': - self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), (-50, 190), - (-10, 190), (-10, 230), (10, 230), (10, 190), (50, 190), - (50, 170), (5, 170), (50, 50), fill = YELLOW, - outline = BLACK) - elif pawn.kind == 'queen': - self.draw_polygon ((-50, 50), (-10, 180), (10, 180), (50, 50), - fill = YELLOW, outline = BLACK) - self.draw_circle ((0, 180), 50, fill = YELLOW) + if pawn.kind == 'tower': + pawns = pawn.tower + else: + pawns = (pawn, ) + for p in pawns: + self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) + if p.kind == 'king': + self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), + (-50, 190), (-10, 190), (-10, 230), (10, 230), + (10, 190), (50, 190), (50, 170), (5, 170), + (50, 50), fill = YELLOW, + outline = BLACK) + elif p.kind == 'queen': + self.draw_polygon ((-50, 50), (-10, 180), (10, 180), + (50, 50), fill = YELLOW, outline = BLACK) + self.draw_circle ((0, 180), 50, fill = YELLOW) + self.trans_translate ((0, 50)) self.trans_pop () def draw (self): diff --git a/host/simu/robots/robospierre/view/robot.py b/host/simu/robots/robospierre/view/robot.py index c0f6624c..90624000 100644 --- a/host/simu/robots/robospierre/view/robot.py +++ b/host/simu/robots/robospierre/view/robot.py @@ -63,7 +63,7 @@ class Robot (simu.inter.drawable.Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.clamp_model.rotation is not None: @@ -77,7 +77,7 @@ class Robot (simu.inter.drawable.Drawable): if load: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (- self.clamp_model.clamping / 47) -- cgit v1.2.3