From 53483bc33fd092d70be211fcaba1db5372e1706d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 16:35:00 +0200 Subject: digital/io-hub: fix bug in head detection workaround --- digital/io-hub/src/robospierre/clamp.c | 25 +++++++++++++++++++++---- digital/io-hub/src/robospierre/logistic.c | 2 +- host/simu/robots/robospierre/model/clamp.py | 2 +- 3 files changed, 23 insertions(+), 6 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; diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index d32edf25..4bbe988f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -206,7 +206,7 @@ class Clamp (Observable): slots[1].pawn is not None or (slots[0].pawn is not None and slots[0].pawn.kind == 'tower')) - slots[2].contact.state = True + slots[2].contact.state = not (slots[2] is not None) if slots[0].pawn: slots[0].codebar.element_type = slots[0].pawn.kind else: -- cgit v1.2.3