summaryrefslogtreecommitdiff
path: root/host/simu/robots/robospierre
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu/robots/robospierre')
-rw-r--r--host/simu/robots/robospierre/link/bag.py8
-rw-r--r--host/simu/robots/robospierre/model/bag.py31
-rw-r--r--host/simu/robots/robospierre/model/clamp.py164
-rw-r--r--host/simu/robots/robospierre/view/bag.py4
-rw-r--r--host/simu/robots/robospierre/view/clamp.py41
-rw-r--r--host/simu/robots/robospierre/view/robot.py4
6 files changed, 198 insertions, 54 deletions
diff --git a/host/simu/robots/robospierre/link/bag.py b/host/simu/robots/robospierre/link/bag.py
index ac68889a..e3368d55 100644
--- a/host/simu/robots/robospierre/link/bag.py
+++ b/host/simu/robots/robospierre/link/bag.py
@@ -28,8 +28,8 @@ import mimot.mex
class Bag:
- def __init__ (self, node):
- self.asserv = asserv.mex.Mex (node)
- self.io_hub = io_hub.mex.Mex (node)
- self.mimot = mimot.mex.Mex (node)
+ def __init__ (self, node, instance = 'robot0'):
+ self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance)
+ self.io_hub = io_hub.mex.Mex (node, '%s:io0' % instance)
+ self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance)
diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py
index ce55f997..cedb3805 100644
--- a/host/simu/robots/robospierre/model/bag.py
+++ b/host/simu/robots/robospierre/model/bag.py
@@ -28,29 +28,36 @@ from simu.model.motor_basic import MotorBasic
from simu.model.distance_sensor_sensopart import DistanceSensorSensopart
from simu.robots.robospierre.model.clamp import Clamp
from math import pi
+import random
class Bag:
def __init__ (self, scheduler, table, link_bag):
- self.color_switch = Switch (link_bag.io_hub.contact[0])
- self.jack = Switch (link_bag.io_hub.contact[1])
+ self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True)
+ self.color_switch.state = random.choice ((False, True))
+ self.color_switch.notify ()
+ self.jack = Switch (link_bag.io_hub.contact[1], invert = True)
+ self.strat_switch = Switch (link_bag.io_hub.contact[9], invert = True)
self.contact = [ Switch (contact)
- for contact in link_bag.io_hub.contact[2:] ]
+ for contact in link_bag.io_hub.contact[2:9] ]
self.position = Position (link_bag.asserv.position)
- self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[0], scheduler,
- 2 * pi, 0, pi)
+ self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler,
+ 8 * pi, 0, pi)
+ self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler,
+ 8 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ]
self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0],
- link_bag.mimot.aux[1], self.clamping_motor)
+ link_bag.mimot.aux[1], self.clamping_motor, self.door_motors,
+ self.contact[0:7], link_bag.io_hub.codebar)
self.distance_sensor = [
DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table,
- (20, -20), -pi * 10 / 180, (self.position, ), 2),
- DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table,
(20, 20), pi * 10 / 180, (self.position, ), 2),
+ DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table,
+ (20, -20), -pi * 10 / 180, (self.position, ), 2),
DistanceSensorSensopart (link_bag.io_hub.adc[2], scheduler, table,
- (-20, 20), pi - pi * 10 / 180, (self.position, ), 2),
- DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table,
(-20, -20), pi + pi * 10 / 180, (self.position, ), 2),
+ DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table,
+ (-20, 20), pi - pi * 10 / 180, (self.position, ), 2),
]
- for adc in link_bag.io_hub.adc[4:]:
- adc.value = 0
+ self.path = link_bag.io_hub.path
+ self.pos_report = link_bag.io_hub.pos_report
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
diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py
index 0b5a85ad..d87cecb9 100644
--- a/host/simu/robots/robospierre/view/bag.py
+++ b/host/simu/robots/robospierre/view/bag.py
@@ -35,6 +35,8 @@ class Bag:
self.jack = Switch (sensor_frame, model_bag.jack, 'Jack')
self.color_switch = Switch (sensor_frame, model_bag.color_switch,
'Color')
+ self.strat_switch = Switch (sensor_frame, model_bag.strat_switch,
+ 'Strat')
self.robot = Robot (table, model_bag.position, model_bag.clamp)
self.clamp = (
ClampTop (actuator_view.add_view (ClampTop.width,
@@ -43,4 +45,6 @@ class Bag:
ClampSide.height), model_bag.clamp))
self.distance_sensor = [DistanceSensorUS (self.robot, ds)
for ds in model_bag.distance_sensor]
+ self.path = Path (table, model_bag.path)
+ self.pos_report = PosReport (table, model_bag.pos_report)
diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py
index bd5f6dab..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):
@@ -133,6 +140,7 @@ class ClampSide (Drawable):
self.draw_pawn ((-slot.x, slot.z), slot.pawn)
# Draw clamp.
if self.model.rotation is not None:
+ self.trans_push ()
self.trans_translate ((0, self.model.elevation))
m = TransMatrix ()
m.rotate (pi + self.model.rotation)
@@ -169,4 +177,13 @@ class ClampSide (Drawable):
self.draw_pawn ((lcenter, 0), self.model.load)
self.draw_rectangle ((lbase, 10), (lcenter - 103, 40), **attr)
self.draw_rectangle ((rbase, 10), (rtip, 40), **attr)
+ self.trans_pop ()
+ # Draw doors.
+ for slot in self.model.slots:
+ if slot.door_motor is not None:
+ self.trans_push ()
+ self.trans_translate ((-slot.x, slot.z + 50))
+ self.trans_rotate (-0.5 * pi + slot.door_motor.angle)
+ self.draw_line ((0, 0), (40, 0))
+ self.trans_pop ()
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)