summaryrefslogtreecommitdiffhomepage
path: root/host/simu
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu')
-rw-r--r--host/simu/model/switch.py8
-rw-r--r--host/simu/robots/robospierre/model/bag.py7
-rw-r--r--host/simu/robots/robospierre/model/clamp.py36
3 files changed, 32 insertions, 19 deletions
diff --git a/host/simu/model/switch.py b/host/simu/model/switch.py
index 30cc1466..6a06a954 100644
--- a/host/simu/model/switch.py
+++ b/host/simu/model/switch.py
@@ -26,13 +26,17 @@ from utils.observable import Observable
class Switch (Observable):
- def __init__ (self, link):
+ def __init__ (self, link, invert = False):
Observable.__init__ (self)
self.link = link
self.state = None
+ self.invert = invert
self.register (self.__update)
def __update (self):
- self.link.state = self.state
+ if not self.invert:
+ self.link.state = self.state
+ else:
+ self.link.state = not self.state
self.link.notify ()
diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py
index 0a89d00c..5a6fa023 100644
--- a/host/simu/robots/robospierre/model/bag.py
+++ b/host/simu/robots/robospierre/model/bag.py
@@ -32,8 +32,8 @@ from math import pi
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.jack = Switch (link_bag.io_hub.contact[1], invert = True)
self.contact = [ Switch (contact)
for contact in link_bag.io_hub.contact[2:] ]
self.position = Position (link_bag.asserv.position)
@@ -42,7 +42,8 @@ class Bag:
self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler,
2 * 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, self.door_motors)
+ link_bag.mimot.aux[1], self.clamping_motor, self.door_motors,
+ self.contact[0:7])
self.distance_sensor = [
DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table,
(20, 20), pi * 10 / 180, (self.position, ), 2),
diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py
index 9bb8bacc..e2592aa8 100644
--- a/host/simu/robots/robospierre/model/clamp.py
+++ b/host/simu/robots/robospierre/model/clamp.py
@@ -29,13 +29,19 @@ from math import pi, cos, sin
class Slot:
"""Slot which can contain a pawn."""
- def __init__ (self, x, y, z, side, door_motor):
+ def __init__ (self, x, y, z, side, door_motor, contact):
self.x = x
self.y = y
self.z = z
self.side = side
- self.pawn = None
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 ()
class Clamp (Observable):
@@ -60,7 +66,7 @@ class Clamp (Observable):
SLOT_SIDE = 6
def __init__ (self, table, robot_position, elevation_motor,
- rotation_motor, clamping_motor, door_motors):
+ rotation_motor, clamping_motor, door_motors, slot_contacts):
Observable.__init__ (self)
self.table = table
self.robot_position = robot_position
@@ -71,19 +77,19 @@ class Clamp (Observable):
door_motors[2], None, door_motors[3], None)
self.slots = (
Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0,
- door_motors[0]),
+ door_motors[0], slot_contacts[0]),
Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0,
- None),
+ None, slot_contacts[1]),
Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0,
- door_motors[1]),
+ door_motors[1], slot_contacts[2]),
Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1,
- door_motors[2]),
+ door_motors[2], slot_contacts[3]),
Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1,
- None),
+ None, slot_contacts[4]),
Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1,
- door_motors[3]),
+ door_motors[3], slot_contacts[5]),
Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None,
- None))
+ None, slot_contacts[6]))
self.load = None
self.robot_position.register (self.__robot_position_notified)
self.elevation_motor.register (self.__elevation_notified)
@@ -98,7 +104,7 @@ class Clamp (Observable):
if slot.pawn is None:
p = self.__get_floor_elements (slot.side)
if p is not None:
- slot.pawn = p
+ slot.set_pawn (p)
p.pos = None
p.notify ()
changed = True
@@ -133,14 +139,16 @@ 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.set_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.set_pawn (self.load)
+ self.load = None
self.notify ()
def __get_floor_elements (self, side):