From 456d1d6670a1a1cf6a01dbbf718a84cfa0a0f34c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 21 Sep 2011 20:07:10 +0200 Subject: digital/io-hub/src/robospierre: go to green zone several times --- digital/io-hub/src/robospierre/element.c | 29 +++++++++++++++++------------ digital/io-hub/src/robospierre/element.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 21 +++++++++++++++++++++ 3 files changed, 42 insertions(+), 12 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 7319adbb..881e1be6 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -675,18 +675,6 @@ element_down (uint8_t element_id, uint8_t element_type) element_set (i, ie); } } - - /* Remove our green zone score at first unload. */ - for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) - { - element_t e = element_get (i); - if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || - (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) - { - e.bonus_load = 0; - element_set (i, e); - } - } } void @@ -821,3 +809,20 @@ element_i_like_green () } } } + +void +element_no_more_green (void) +{ + uint8_t i; + /* Remove our green zone score at first unload. */ + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) + { + element_t e = element_get (i); + if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + { + e.bonus_load = 0; + element_set (i, e); + } + } +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 243f5b94..41985e95 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -206,4 +206,8 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab, uint8_t escape); void element_i_like_green (); +/** Do not go to our green zone any more. */ +void +element_no_more_green (void); + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7732fb50..93fb447f 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -90,6 +90,8 @@ struct top_t uint8_t broken; /** Saved, direction when picking element. */ uint8_t go_to_element_direction; + /** Return to our green zone again. */ + uint8_t green_again; }; /** Global context. */ @@ -99,6 +101,7 @@ struct top_t top_global; FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1) { element_init (); + ctx.green_again = 3; asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), PG_Y (PG_LENGTH - 200), 0); return FSM_NEXT (TOP_START, init_start_round); @@ -305,6 +308,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, element_failure (ctx.target_element_id); /* Do not take this one again. */ if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); + if (ctx.green_again) + { + ctx.green_again--; + if (ctx.green_again == 0) + element_no_more_green (); + } switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); @@ -321,6 +330,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, element_failure (ctx.target_element_id); if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working); + if (ctx.green_again) + { + ctx.green_again--; + if (ctx.green_again == 0) + element_no_more_green (); + } switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop); @@ -331,6 +346,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) { move_stop (); + if (ctx.green_again) + { + ctx.green_again--; + if (ctx.green_again == 0) + element_no_more_green (); + } return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); } -- cgit v1.2.3