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/top.c | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) (limited to 'digital/io-hub/src/robospierre/top.c') 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