summaryrefslogtreecommitdiff
path: root/host/simu/robots/robospierre/model/clamp.py
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu/robots/robospierre/model/clamp.py')
-rw-r--r--host/simu/robots/robospierre/model/clamp.py164
1 files changed, 140 insertions, 24 deletions
diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py
index ff27a7ea..4bbe988f 100644
--- a/host/simu/robots/robospierre/model/clamp.py
+++ b/host/simu/robots/robospierre/model/clamp.py
@@ -24,16 +24,20 @@
"""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:
"""Slot which can contain a pawn."""
- def __init__ (self, x, y, z, side):
+ def __init__ (self, x, y, z, side, door_motor, contact, codebar = None):
self.x = x
self.y = y
self.z = z
self.side = side
+ self.door_motor = door_motor
+ self.contact = contact
+ self.codebar = codebar
self.pawn = None
class Clamp (Observable):
@@ -42,7 +46,7 @@ class Clamp (Observable):
ELEVATION_MOTOR_STROKE = 120.0 * 5.0 / 6.0
ROTATION_STROKE = pi
- ROTATION_MOTOR_STROKE = pi * 115.0 / 12.0
+ ROTATION_MOTOR_STROKE = 2 * pi * 36.088 / 16
CLAMPING_STROKE = 10
CLAMPING_MOTOR_STROKE = pi
@@ -59,21 +63,41 @@ class Clamp (Observable):
SLOT_SIDE = 6
def __init__ (self, table, robot_position, elevation_motor,
- rotation_motor, clamping_motor):
+ rotation_motor, clamping_motor, door_motors, slot_contacts,
+ codebars):
Observable.__init__ (self)
self.table = table
self.robot_position = robot_position
self.elevation_motor = elevation_motor
self.rotation_motor = rotation_motor
self.clamping_motor = clamping_motor
+ self.rotation_motor.limits.min = 0
+ self.rotation_motor.limits.notify ()
+ self.door_motors = (door_motors[0], None, door_motors[1],
+ door_motors[2], None, door_motors[3], None)
self.slots = (
- Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0),
- Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0),
- Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0),
- Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1),
- Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1),
- Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1),
- Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None))
+ Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0,
+ door_motors[0], slot_contacts[0], codebars[0]),
+ Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0,
+ None, slot_contacts[1]),
+ Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0,
+ door_motors[1], slot_contacts[2]),
+ Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1,
+ door_motors[2], slot_contacts[3], codebars[1]),
+ Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1,
+ None, slot_contacts[4]),
+ Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1,
+ 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)
@@ -81,18 +105,34 @@ 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 ()
self.notify ()
def __elevation_notified (self):
@@ -123,29 +163,74 @@ class Clamp (Observable):
if self.clamping == 0 and self.load is None:
# Load an element.
slot = self.__get_clamp_slot ()
- if slot:
- self.load, slot.pawn = slot.pawn, self.load
+ if slot and slot.pawn is not None:
+ self.load = slot.pawn
+ 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:
- self.load, slot.pawn = slot.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
+ 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)
+ slots[2].pawn = None
+ if slots[0].pawn is None and slots[1].pawn is None \
+ and slots[2].pawn and slots[2].door_motor.angle:
+ slots[0].pawn, 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'))
+ slots[2].contact.state = not (slots[2] is not None)
+ if slots[0].pawn:
+ slots[0].codebar.element_type = slots[0].pawn.kind
+ else:
+ slots[0].codebar.element_type = None
+ slots[0].codebar.notify ()
+ 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 and clamp_slot != self.SLOT_FRONT_TOP \
+ and clamp_slot != self.SLOT_BACK_TOP:
+ 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."""
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
+ xmargin = 40
ymargin = 50
for o in self.table.obstacles:
if o.level == 1 and o.pos is not None:
@@ -170,3 +255,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