From cff3a91df81737d5f9ce2173375b719ff21d0969 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 00:41:24 +0200 Subject: digital/io-hub: drop the right element type --- digital/io-hub/src/robospierre/element.c | 4 ++-- digital/io-hub/src/robospierre/logistic.c | 19 +++++++++++++++++++ digital/io-hub/src/robospierre/logistic.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 4 +++- 4 files changed, 28 insertions(+), 3 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 37efa46f..4c669c1c 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -742,7 +742,7 @@ uint8_t element_blocking (uint8_t element_id) { element_t e = element_get (element_id); - return e.type == ELEMENT_TOWER; + return e.type == ELEMENT_TOWER || e.type == ELEMENT_PAWN; } uint8_t @@ -754,7 +754,7 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab) for (i = 0; i < UTILS_COUNT (element_table); i++) { e = element_get (i); - if (e.type == ELEMENT_TOWER) + if (e.type == ELEMENT_TOWER || e.type == ELEMENT_PAWN) { /* Compute square of distance to obstacle, see * distance_segment_point in modules/math/geometry for the method diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 629ee0c1..db0c8b94 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -562,6 +562,25 @@ logistic_drop (uint8_t direction) logistic_decision (); } +uint8_t +logistic_drop_element_type (uint8_t direction) +{ + uint8_t bay = direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + uint8_t nb = 0; + uint8_t element_type = ELEMENT_NONE; + uint8_t i; + for (i = bay; i < bay + 3; i++) + { + if (ctx.slots[i]) + { + nb++; + element_type = ctx.slots[i]; + } + } + return nb > 1 ? ELEMENT_TOWER : element_type; +} + void logistic_dump (void) { diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 4a06dcb7..0a076477 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -128,6 +128,10 @@ logistic_element_move_done (void); void logistic_drop (uint8_t direction); +/** Get element type to be dropped. */ +uint8_t +logistic_drop_element_type (uint8_t direction); + /** Dump every element. */ void logistic_dump (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 3313a908..f1f14cdd 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -405,7 +405,9 @@ FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { if (ctx.target_element_id != 0xff) - element_down (ctx.target_element_id, ELEMENT_TOWER); + element_down (ctx.target_element_id, + logistic_drop_element_type + (logistic_global.collect_direction)); if (!IO_GET (CONTACT_STRAT)) element_i_like_green (); asserv_move_linearly (logistic_global.collect_direction -- cgit v1.2.3