summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-03 04:37:35 +0200
committerNicolas Schodet2011-06-03 06:03:49 +0200
commit4eb705e639457a07e6d5e9423ba81f8cede1b979 (patch)
tree62a0af85401c4751b7b5b091d080d6d19f0f027a
parent6ef0e7de88e2c4896ebdbd2ed1617ded5c89c31c (diff)
digital/io-hub: handle element change to head
-rw-r--r--digital/io-hub/src/robospierre/clamp.c39
-rw-r--r--digital/io-hub/src/robospierre/logistic.c8
-rw-r--r--digital/io-hub/src/robospierre/logistic.h4
-rw-r--r--host/simu/robots/robospierre/model/clamp.py6
4 files changed, 52 insertions, 5 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index e05f79d7..6b48c56a 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -373,11 +373,17 @@ clamp_route (void)
}
else if (pos_current == CLAMP_BAY_FRONT_LEAVE)
{
- pos_new = CLAMP_BAY_FRONT_LEAVING;
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request))
+ pos_new = pos_request;
+ else
+ pos_new = CLAMP_BAY_FRONT_LEAVING;
}
else if (pos_current == CLAMP_BAY_BACK_LEAVE)
{
- pos_new = CLAMP_BAY_BACK_LEAVING;
+ if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_request))
+ pos_new = pos_request;
+ else
+ pos_new = CLAMP_BAY_BACK_LEAVING;
}
else if (pos_current == CLAMP_BAY_FRONT_LEAVING)
{
@@ -417,6 +423,34 @@ clamp_route (void)
ctx.pos_current = pos_new;
}
+/* When lifting an element, we can discover it is actually a head. In this
+ * case, change destination. */
+void
+clamp_change (void)
+{
+ uint8_t from = logistic_global.moving_from;
+ if (logistic_global.slots[from] == ELEMENT_PAWN)
+ {
+ /* Look head contact. */
+ uint8_t contact_head = 0;
+ if (from == CLAMP_SLOT_FRONT_BOTTOM)
+ contact_head = !IO_GET (CONTACT_FRONT_TOP);
+ else if (from == CLAMP_SLOT_BACK_BOTTOM)
+ contact_head = !IO_GET (CONTACT_BACK_TOP);
+ /* Change? */
+ if (contact_head)
+ {
+ logistic_element_change (from, ELEMENT_KING);
+ if (logistic_global.moving_from != from)
+ /* Cancel move. */
+ ctx.pos_request = ctx.moving_to = from;
+ else
+ /* Change move. */
+ ctx.pos_request = ctx.moving_to = logistic_global.moving_to;
+ }
+ }
+}
+
static void
clamp_blocked (void)
{
@@ -861,6 +895,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success,
done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING,
next, CLAMP_MOVE_DST_ROUTING)
{
+ clamp_change ();
if (ctx.pos_current == ctx.pos_request)
{
if (clamp_slot_door[ctx.pos_current] != 0xff)
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
index dea6e0a5..f59f2da4 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -534,6 +534,14 @@ logistic_element_new (uint8_t pos, uint8_t element_type)
}
void
+logistic_element_change (uint8_t pos, uint8_t element_type)
+{
+ assert (pos < CLAMP_SLOT_NB);
+ ctx.slots[pos] = element_type;
+ logistic_decision ();
+}
+
+void
logistic_element_move_done (void)
{
assert (!ctx.slots[ctx.moving_to]);
diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h
index 6acb681e..4a06dcb7 100644
--- a/digital/io-hub/src/robospierre/logistic.h
+++ b/digital/io-hub/src/robospierre/logistic.h
@@ -115,6 +115,10 @@ logistic_update (void);
void
logistic_element_new (uint8_t pos, uint8_t element_type);
+/** Oh la la, the pawn was not a pawn, it's a head. */
+void
+logistic_element_change (uint8_t pos, uint8_t element_type);
+
/** To be called when a element movement is done. */
void
logistic_element_move_done (void);
diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py
index dbcab5f2..c199259e 100644
--- a/host/simu/robots/robospierre/model/clamp.py
+++ b/host/simu/robots/robospierre/model/clamp.py
@@ -203,8 +203,7 @@ class Clamp (Observable):
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)
+ slots[2].contact.state = True
if slots[0].pawn:
slots[0].codebar.element_type = slots[0].pawn.kind
else:
@@ -213,7 +212,8 @@ class Clamp (Observable):
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:
+ 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 ()