summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-03 16:35:00 +0200
committerNicolas Schodet2011-06-03 16:35:00 +0200
commit53483bc33fd092d70be211fcaba1db5372e1706d (patch)
tree5e7c2dec4594b4533ef42856600eb21c9a756372 /digital/io-hub/src/robospierre
parente41b10c7d7ddeb9560848b63859b58614d1362ef (diff)
digital/io-hub: fix bug in head detection workaround
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c25
-rw-r--r--digital/io-hub/src/robospierre/logistic.c2
2 files changed, 22 insertions, 5 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index f91af6a0..a74c643c 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -161,6 +161,8 @@ struct clamp_t
uint8_t open;
/** True if clamp position is controled. */
uint8_t controled;
+ /** Contact state at start of move, for head check. */
+ uint8_t contact_head_before_move;
};
/** Global context. */
@@ -212,6 +214,9 @@ clamp_openclose (uint8_t open);
static void
clamp_route (void);
+static void
+clamp_head_check_prepare (uint8_t from);
+
void
clamp_init (void)
{
@@ -243,6 +248,7 @@ clamp_move_element (uint8_t from, uint8_t to)
assert (from != to);
ctx.pos_request = from;
ctx.moving_to = to;
+ clamp_head_check_prepare (from);
FSM_HANDLE (AI, clamp_move);
}
@@ -435,11 +441,11 @@ clamp_taken_pawn (uint8_t element_type)
/* When lifting an element, we can discover it is actually a head. In this
* case, change destination. */
-void
-clamp_change (void)
+static void
+clamp_head_check (void)
{
uint8_t from = logistic_global.moving_from;
- if (logistic_global.slots[from] == ELEMENT_PAWN)
+ if (!ctx.contact_head_before_move && logistic_global.slots[from] == ELEMENT_PAWN)
{
/* Look head contact. */
uint8_t contact_head = 0;
@@ -462,6 +468,17 @@ clamp_change (void)
}
}
+/** Prepare head check, if contact is set yet, this is not a new head. */
+static void
+clamp_head_check_prepare (uint8_t from)
+{
+ ctx.contact_head_before_move = 0;
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (from))
+ ctx.contact_head_before_move = !IO_GET (CONTACT_FRONT_TOP);
+ else if (CLAMP_IS_SLOT_IN_BACK_BAY (from))
+ ctx.contact_head_before_move = !IO_GET (CONTACT_BACK_TOP);
+}
+
static void
clamp_blocked (void)
{
@@ -909,7 +926,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 ();
+ clamp_head_check ();
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 f59f2da4..f9b13427 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -544,7 +544,7 @@ logistic_element_change (uint8_t pos, uint8_t element_type)
void
logistic_element_move_done (void)
{
- assert (!ctx.slots[ctx.moving_to]);
+ assert (!ctx.slots[ctx.moving_to] || ctx.moving_to == ctx.moving_from);
ctx.slots[ctx.moving_to] = ctx.slots[ctx.moving_from];
ctx.slots[ctx.moving_from] = 0;
ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB;