From 4560ca69f29acf76eaf3dc5dbadf9f3287108aca Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 15 May 2010 07:28:21 +0200 Subject: digital/io/src: try to unblock, but give up if loader always blocked --- digital/io/src/ai_move_cb.c | 106 ++++++++++++++++++++++++++++++++++++++------ digital/io/src/move.fsm | 31 ++++++++++--- digital/io/src/move.h | 2 + 3 files changed, 120 insertions(+), 19 deletions(-) diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index 0c54de7c..5fc70653 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -456,11 +456,13 @@ ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void) * MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=> * => MOVE_LOADER_UNBLOCKING_DOWNING * loader down + * reset unblocking retry counter */ fsm_branch_t ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void) { loader_down (); + move_data.loader_unblocking_retry = 2; return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed); } @@ -468,44 +470,122 @@ ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void) * MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=> * => MOVE_LOADER_UNBLOCKING_DOWNING * loader down + * reset unblocking retry counter */ fsm_branch_t ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void) { loader_down (); + move_data.loader_unblocking_retry = 2; return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed); } /* * MOVE_LOADER_UNBLOCKING_DOWNING =loader_downed=> - * rotate => MOVE_ROTATING - * repeat last rotate - * move => MOVE_MOVING - * repeat last move + * path_found_rotate => MOVE_ROTATING + * rotate towards next position. + * path_found => MOVE_MOVING + * move to next position. + * no_path_found => MOVE_IDLE + * post failure. */ fsm_branch_t ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_downed (void) { - if (move_go_or_rotate (move_data.step, move_data.step_angle, - move_data.step_with_angle, - move_data.step_backward) == 2) - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, rotate); + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate); + else + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found); + } else - return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, move); + { + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found); + } } /* * MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=> - * => MOVE_LOADER_UNBLOCKING_UPING + * tryagain => MOVE_LOADER_UNBLOCKING_UPING * move backward * loader up + * tryout_path_found_rotate => MOVE_ROTATING + * rotate towards next position. + * tryout_path_found => MOVE_ROTATING + * move to next position. + * tryout_no_path_found => MOVE_IDLE + * post failure. */ fsm_branch_t ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_errored (void) { - asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); - loader_up (); - return ai_next (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored); + if (--move_data.loader_unblocking_retry) + { + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain); + } + else + { + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate); + else + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found); + } + else + { + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found); + } + } +} + +/* + * MOVE_LOADER_UNBLOCKING_DOWNING =state_timeout=> + * tryagain => MOVE_LOADER_UNBLOCKING_UPING + * move backward + * loader up + * tryout_path_found_rotate => MOVE_ROTATING + * rotate towards next position. + * tryout_path_found => MOVE_ROTATING + * move to next position. + * tryout_no_path_found => MOVE_IDLE + * post failure. + */ +fsm_branch_t +ai__MOVE_LOADER_UNBLOCKING_DOWNING__state_timeout (void) +{ + if (--move_data.loader_unblocking_retry) + { + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryagain); + } + else + { + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found_rotate); + else + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found); + } + else + { + main_post_event (AI_EVENT_move_fsm_failed); + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_no_path_found); + } + } } diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index abedd8dd..fad6b6b7 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -18,7 +18,7 @@ States: waiting for obstacle to disappear. MOVE_LOADER_UNBLOCKING_UPING moving loader up and move backward to unblock loader. - MOVE_LOADER_UNBLOCKING_DOWNING + MOVE_LOADER_UNBLOCKING_DOWNING [timeout=450] moving loader down. Events: @@ -93,15 +93,34 @@ MOVE_WAIT_FOR_CLEAR_PATH: MOVE_LOADER_UNBLOCKING_UPING: bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING loader down + reset unblocking retry counter bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING loader down + reset unblocking retry counter MOVE_LOADER_UNBLOCKING_DOWNING: - loader_downed: rotate -> MOVE_ROTATING - repeat last rotate - loader_downed: move -> MOVE_MOVING - repeat last move - loader_errored -> MOVE_LOADER_UNBLOCKING_UPING + loader_downed: path_found_rotate -> MOVE_ROTATING + rotate towards next position. + loader_downed: path_found -> MOVE_MOVING + move to next position. + loader_downed: no_path_found -> MOVE_IDLE + post failure. + loader_errored: tryagain -> MOVE_LOADER_UNBLOCKING_UPING + move backward + loader up + loader_errored: tryout_path_found_rotate -> MOVE_ROTATING + rotate towards next position. + loader_errored: tryout_path_found -> MOVE_ROTATING + move to next position. + loader_errored: tryout_no_path_found -> MOVE_IDLE + post failure. + state_timeout: tryagain -> MOVE_LOADER_UNBLOCKING_UPING move backward loader up + state_timeout: tryout_path_found_rotate -> MOVE_ROTATING + rotate towards next position. + state_timeout: tryout_path_found -> MOVE_ROTATING + move to next position. + state_timeout: tryout_no_path_found -> MOVE_IDLE + post failure. diff --git a/digital/io/src/move.h b/digital/io/src/move.h index 7f860802..6078a1e3 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -68,6 +68,8 @@ struct move_data_t uint8_t final_move; /** Distance to remove from path. */ int16_t shorten; + /** Loader unblocking retry counter. */ + uint8_t loader_unblocking_retry; }; /** -- cgit v1.2.3