summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-14 01:28:54 +0200
committerNicolas Schodet2011-05-14 01:38:19 +0200
commitc8d4bd0e0f79dc8e49ad0074a1e67317e903c41f (patch)
tree9a2b010211a2382f7dfe3812a907674a969fb75d
parenta3fdbecac564a600c65568cac0e02fc72604567e (diff)
digital/io-hub, host/simu: add contacts
-rw-r--r--digital/io-hub/src/robospierre/contact_defs.h18
-rw-r--r--digital/io-hub/src/robospierre/simu.host.c2
-rw-r--r--digital/io-hub/src/robospierre/simu.host.h2
-rw-r--r--digital/io-hub/tools/io_hub/mex.py2
-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
7 files changed, 49 insertions, 26 deletions
diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h
index 55b34354..1e04f6bf 100644
--- a/digital/io-hub/src/robospierre/contact_defs.h
+++ b/digital/io-hub/src/robospierre/contact_defs.h
@@ -27,11 +27,21 @@
#define CONTACT_COLOR A, 7
#define CONTACT_JACK F, 7
-#define CONTACT_EX1 E, 0
-#define CONTACT_EX2 E, 1
+#define CONTACT_FRONT_BOTTOM A, 4
+#define CONTACT_FRONT_MIDDLE F, 4
+#define CONTACT_BACK_BOTTOM A, 5
+#define CONTACT_BACK_MIDDLE F, 5
+#define CONTACT_FRONT_TOP A, 6
+#define CONTACT_BACK_TOP F, 6
+#define CONTACT_SIDE E, 7
#define CONTACT_LIST \
- CONTACT (CONTACT_EX1) \
- CONTACT (CONTACT_EX2)
+ CONTACT (CONTACT_FRONT_BOTTOM) \
+ CONTACT (CONTACT_FRONT_MIDDLE) \
+ CONTACT (CONTACT_FRONT_TOP) \
+ CONTACT (CONTACT_BACK_BOTTOM) \
+ CONTACT (CONTACT_BACK_MIDDLE) \
+ CONTACT (CONTACT_BACK_TOP) \
+ CONTACT (CONTACT_SIDE)
#endif /* contact_defs_h */
diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c
index 8f4019aa..281a60d6 100644
--- a/digital/io-hub/src/robospierre/simu.host.c
+++ b/digital/io-hub/src/robospierre/simu.host.c
@@ -31,7 +31,7 @@
#include "io.h"
/** AVR registers. */
-uint8_t PINE;
+uint8_t PINA, PINE, PINF;
/** Initialise simulation. */
void
diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h
index 4b461e86..ed6a002d 100644
--- a/digital/io-hub/src/robospierre/simu.host.h
+++ b/digital/io-hub/src/robospierre/simu.host.h
@@ -27,7 +27,7 @@
#ifdef HOST
-extern uint8_t PINE;
+extern uint8_t PINA, PINE, PINF;
#else /* !defined (HOST) */
diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py
index 7c8c0012..d9568197 100644
--- a/digital/io-hub/tools/io_hub/mex.py
+++ b/digital/io-hub/tools/io_hub/mex.py
@@ -31,7 +31,7 @@ ADC_NB = 8
PWM_NB = 6
PWM_VALUE_MAX = 1024
-CONTACT_NB = 4
+CONTACT_NB = 9
CONTACT_INIT = 0xffffffff
class Mex:
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):