summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 13:38:25 +0200
committerNicolas Schodet2011-06-03 23:36:53 +0200
commit0ee500d39cf65c776b02d26bd44d7c0e8fcfb8b7 (patch)
treed7083a90d6f64926c8e8e1d6a29df77107f29cbe /digital/io-hub/src/robospierre
parent5c915c07f8440097109763848d4fc0e06ea8078d (diff)
digital/io-hub: handle tower detection
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c36
-rw-r--r--digital/io-hub/src/robospierre/logistic.c22
2 files changed, 51 insertions, 7 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index a74c643c..2340b54f 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -479,6 +479,33 @@ clamp_head_check_prepare (uint8_t from)
ctx.contact_head_before_move = !IO_GET (CONTACT_BACK_TOP);
}
+/* When clamp moved to bottom slot, we can discover it is actually a tower.
+ * In this case, stop movement. */
+static uint8_t
+clamp_tower_check (void)
+{
+ uint8_t from = logistic_global.moving_from;
+ if ((from == CLAMP_SLOT_FRONT_BOTTOM || from == CLAMP_SLOT_BACK_BOTTOM)
+ && ctx.pos_current == from
+ && logistic_global.slots[from] == ELEMENT_PAWN)
+ {
+ /* Look tower contact. */
+ uint8_t contact_tower;
+ if (from == CLAMP_SLOT_FRONT_BOTTOM)
+ contact_tower = !IO_GET (CONTACT_FRONT_MIDDLE);
+ else
+ contact_tower = !IO_GET (CONTACT_BACK_MIDDLE);
+ /* Change? */
+ if (contact_tower)
+ {
+ logistic_element_change (from, ELEMENT_TOWER);
+ clamp_taken_pawn (ELEMENT_TOWER);
+ return 1;
+ }
+ }
+ return 0;
+}
+
static void
clamp_blocked (void)
{
@@ -869,10 +896,17 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure,
}
FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
+ cancel, CLAMP_MOVE_IDLE,
done, CLAMP_MOVE_SRC_CLAMP_CLOSING,
next, CLAMP_MOVE_SRC_ROUTING)
{
- if (ctx.pos_current == ctx.pos_request)
+ if (clamp_tower_check ())
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
+ cancel);
+ }
+ else if (ctx.pos_current == ctx.pos_request)
{
clamp_openclose (0);
return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
index f9b13427..629ee0c1 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -574,12 +574,22 @@ logistic_dump (void)
static uint8_t
logistic_slot_clear (uint8_t slot)
{
- if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot)
- && ctx.slots[CLAMP_SLOT_FRONT_MIDDLE])
- return 0;
- if (CLAMP_IS_SLOT_IN_BACK_BAY (slot)
- && ctx.slots[CLAMP_SLOT_BACK_MIDDLE])
- return 0;
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot))
+ {
+ if (ctx.slots[CLAMP_SLOT_FRONT_MIDDLE])
+ return 0;
+ uint8_t middle_type = ctx.slots[CLAMP_SLOT_FRONT_BOTTOM];
+ if (ELEMENT_IS_HEAD (middle_type) || middle_type == ELEMENT_TOWER)
+ return 0;
+ }
+ else if (CLAMP_IS_SLOT_IN_BACK_BAY (slot))
+ {
+ if (ctx.slots[CLAMP_SLOT_BACK_MIDDLE])
+ return 0;
+ uint8_t middle_type = ctx.slots[CLAMP_SLOT_BACK_BOTTOM];
+ if (ELEMENT_IS_HEAD (middle_type) || middle_type == ELEMENT_TOWER)
+ return 0;
+ }
return 1;
}