From a20005b026befa8499c1fca9f464e2d0300700ce Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 14 May 2012 10:56:00 +0200 Subject: digital/io-hub/src/guybrush: add clamp_request to put a end to sync problems --- digital/io-hub/src/guybrush/bottom_clamp.c | 61 ++++++++++++++++++++++++++++-- 1 file changed, 57 insertions(+), 4 deletions(-) (limited to 'digital/io-hub/src/guybrush/bottom_clamp.c') diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c index 0b951138..f801a78e 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.c +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -213,6 +213,8 @@ struct clamp_t uint8_t calm_mode; /** Compting the number of time the bottom clamp is being blocked in a row (reset when going back to idle)*/ uint8_t cpt_blocked; + /** Request from top FSM to be processed once in IDLE state. */ + uint16_t idle_request; }; /*Global context. */ @@ -236,6 +238,18 @@ void clamp_calm_mode(int mode) ctx.calm_mode=mode; } +void +clamp_request (uint16_t event) +{ + /* If event can not be handled right now, keep it until going in IDLE + * state. */ + if (!FSM_HANDLE_VAR (AI, event)) + { + assert (!ctx.idle_request); + ctx.idle_request = event; + } +} + uint8_t clamp_read_blocked_cpt(void) { return ctx.cpt_blocked; @@ -312,17 +326,56 @@ FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_success, CLAMP_WAIT_BEFORE_IDL /*---------------------------------------------------------*/ /* IDLE!! */ /*---------------------------------------------------------*/ -FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE, CLAMP_IDLE) +FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE, + request_clean_start, CLAMP_READY_TO_CLEAN, + request_tree_detected, CLAMP_BOTTOM_CLAMP_HIDE_POS, + idle, CLAMP_IDLE) { - /*Going back to the idle position, ready for showtime.*/ - fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); - return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE); + if (ctx.idle_request) + { + uint16_t idle_request = ctx.idle_request; + ctx.idle_request = 0; + if (idle_request == FSM_EVENT (AI, clean_start)) + { + fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); + return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, + request_clean_start); + } + else if (idle_request == FSM_EVENT (AI, tree_detected)) + { + /* Hidding the clamp inside the robot. */ + if (ctx.clamp_1_down) + { + move_needed (HIDE_POS_TREE * 250, FAST_ROTATION); + } + else + { + move_needed ((HALF_TURN + HIDE_POS_TREE) * 250, FAST_ROTATION); + ctx.clamp_1_down = 1; + } + return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, + request_tree_detected); + } + else + { + assert (0); + return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, idle); + } + } + else + { + /* Going back to the idle position, ready for showtime. */ + fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); + return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, idle); + } } + /*---------------------------------------------------------*/ /* parts of the FSM that Clean areas */ /*---------------------------------------------------------*/ FSM_TRANS (CLAMP_IDLE, clean_start,CLAMP_READY_TO_CLEAN) { + fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); return FSM_NEXT(CLAMP_IDLE,clean_start); } -- cgit v1.2.3