summaryrefslogtreecommitdiff
path: root/digital/io-hub/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src')
-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
3 files changed, 49 insertions, 2 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);