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 ++++++++++++++++++++++++++++-- digital/io-hub/src/guybrush/bottom_clamp.h | 4 ++ digital/io-hub/src/guybrush/top.c | 47 +++++------------------ 3 files changed, 71 insertions(+), 41 deletions(-) (limited to 'digital') 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); } diff --git a/digital/io-hub/src/guybrush/bottom_clamp.h b/digital/io-hub/src/guybrush/bottom_clamp.h index c3cd773a..240b1579 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.h +++ b/digital/io-hub/src/guybrush/bottom_clamp.h @@ -31,5 +31,9 @@ void clamp_calm_mode(int mode); /*function to find out how many times the clamp has been blocked*/ uint8_t clamp_read_blocked_cpt(void); +/** Request a specific move to be executed now or as soon as going to IDLE + * state. */ +void +clamp_request (uint16_t event); #endif /* bottom_clamp_h */ diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 0fcf9b03..0b763480 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -37,6 +37,7 @@ #include "strat.h" #include "path.h" +#include "bottom_clamp.h" #include "output_defs.h" @@ -58,7 +59,7 @@ FSM_STATES ( /* Going to a collect position above or below a totem. */ TOP_TOTEM_GOING, /* Cleaning: waiting clamp FSM. */ - TOP_TOTEM_CLEAN_WAITING, + TOP_TOTEM_CLEAN_STARTING, /* Cleaning: approaching totem. */ TOP_TOTEM_CLEAN_APPROACHING, /* Cleaning; closing clamps. */ @@ -67,8 +68,6 @@ FSM_STATES ( TOP_TOTEM_CLEAN_GOING_BACK, /* Cleaning: loading. */ TOP_TOTEM_CLEAN_LOADING, - /* Waiting clamp FSM to put clamps down. */ - TOP_TOTEM_CLAMP_WAITING, /* Put clamps down. */ TOP_TOTEM_CLAMP_DOWNING, /* Approaching a totem. */ @@ -204,27 +203,16 @@ FSM_TRANS (TOP_INIT, init_start_round, /** TOTEM */ -FSM_TRANS (TOP_TOTEM_GOING, move_success, - wait, TOP_TOTEM_CLEAN_WAITING, - ready, TOP_TOTEM_CLEAN_APPROACHING) +FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLEAN_STARTING) { - if (!FSM_CAN_HANDLE (AI, clean_start)) - { - return FSM_NEXT (TOP_TOTEM_GOING, move_success, wait); - } - else - { - FSM_HANDLE (AI, clean_start); - asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 130); - return FSM_NEXT (TOP_TOTEM_GOING, move_success, ready); - } + clamp_request (FSM_EVENT (AI, clean_start)); + return FSM_NEXT (TOP_TOTEM_GOING, move_success); } -FSM_TRANS (TOP_TOTEM_CLEAN_WAITING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING) +FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING) { - FSM_HANDLE (AI, clean_start); asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 130); - return FSM_NEXT (TOP_TOTEM_CLEAN_WAITING, clamps_ready); + return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamps_ready); } FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success, @@ -248,25 +236,10 @@ FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success, return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success); } -FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready, - wait, TOP_TOTEM_CLAMP_WAITING, - ready, TOP_TOTEM_CLAMP_DOWNING) -{ - if (!FSM_CAN_HANDLE (AI, tree_detected)) - { - return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready, wait); - } - else - { - FSM_HANDLE (AI, tree_detected); - return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready, ready); - } -} - -FSM_TRANS (TOP_TOTEM_CLAMP_WAITING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING) +FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING) { - FSM_HANDLE (AI, tree_detected); - return FSM_NEXT (TOP_TOTEM_CLAMP_WAITING, clamps_ready); + clamp_request (FSM_EVENT (AI, tree_detected)); + return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready); } FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING) -- cgit v1.2.3