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 ++++++++++++++++++++++++++++++++++++++------ 1 file changed, 93 insertions(+), 13 deletions(-) (limited to 'digital/io/src/ai_move_cb.c') 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); + } + } } -- cgit v1.2.3