From 4eb705e639457a07e6d5e9423ba81f8cede1b979 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 04:37:35 +0200 Subject: digital/io-hub: handle element change to head --- digital/io-hub/src/robospierre/clamp.c | 39 +++++++++++++++++++++++++++++-- digital/io-hub/src/robospierre/logistic.c | 8 +++++++ digital/io-hub/src/robospierre/logistic.h | 4 ++++ 3 files changed, 49 insertions(+), 2 deletions(-) (limited to 'digital/io-hub') 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 @@ -533,6 +533,14 @@ logistic_element_new (uint8_t pos, uint8_t element_type) logistic_decision (); } +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) { 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); -- cgit v1.2.3