From df32c7a59f59166a22eff4f2a76ec6244c64d721 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 03:23:08 +0200 Subject: digital/io/src: handle loader blocking --- digital/io/src/ai_move_cb.c | 86 +++++++++++++++++++++++++++++++++++++++++++++ digital/io/src/move.fsm | 26 ++++++++++++++ digital/io/src/top.fsm | 2 +- 3 files changed, 113 insertions(+), 1 deletion(-) (limited to 'digital/io/src') diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index e37b56e6..5544eaf6 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -47,6 +47,8 @@ #include +#define MOVE_LOADER_UNBLOCKING_DISTANCE 70 + /** Go or rotate toward position, returns 1 for linear move, 2 for angular * move. */ static uint8_t @@ -211,6 +213,20 @@ ai__MOVE_ROTATING__bot_move_succeed (void) return ai_next (MOVE_ROTATING, bot_move_succeed); } +/* + * MOVE_ROTATING =loader_errored=> + * => MOVE_LOADER_UNBLOCKING_UPING + * move backward + * loader up + */ +fsm_branch_t +ai__MOVE_ROTATING__loader_errored (void) +{ + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return ai_next (MOVE_ROTATING, loader_errored); +} + /* * MOVE_MOVING =bot_move_succeed=> * done => MOVE_IDLE @@ -282,6 +298,20 @@ ai__MOVE_MOVING__obstacle_in_front (void) return ai_next (MOVE_MOVING, obstacle_in_front); } +/* + * MOVE_MOVING =loader_errored=> + * => MOVE_LOADER_UNBLOCKING_UPING + * move backward + * loader up + */ +fsm_branch_t +ai__MOVE_MOVING__loader_errored (void) +{ + asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE); + loader_up (); + return ai_next (MOVE_MOVING, loader_errored); +} + /* * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=> * path_found_rotate => MOVE_ROTATING @@ -371,4 +401,60 @@ ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void) } } +/* + * MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=> + * => MOVE_LOADER_UNBLOCKING_DOWNING + * loader down + */ +fsm_branch_t +ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void) +{ + loader_down (); + return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed); +} + +/* + * MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=> + * => MOVE_LOADER_UNBLOCKING_DOWNING + * loader down + */ +fsm_branch_t +ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void) +{ + loader_down (); + 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 + */ +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); + else + return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, move); +} + +/* + * MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=> + * => MOVE_LOADER_UNBLOCKING_UPING + * move backward + * loader up + */ +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); +} + diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index 20334686..2812f686 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -16,6 +16,10 @@ States: moving backward to go away from what is blocking the bot. MOVE_WAIT_FOR_CLEAR_PATH [timeout=255] waiting for obstacle to disappear. + MOVE_LOADER_UNBLOCKING_UPING + moving loader up and move backward to unblock loader. + MOVE_LOADER_UNBLOCKING_DOWNING + moving loader down. Events: move_start @@ -34,6 +38,9 @@ MOVE_IDLE: MOVE_ROTATING: bot_move_succeed -> MOVE_MOVING move to next position. + loader_errored -> MOVE_LOADER_UNBLOCKING_UPING + move backward + loader up MOVE_MOVING: bot_move_succeed: done -> MOVE_IDLE @@ -50,6 +57,9 @@ MOVE_MOVING: obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH reset final_move. stop the bot. + loader_errored -> MOVE_LOADER_UNBLOCKING_UPING + move backward + loader up MOVE_MOVING_BACKWARD_TO_TURN_FREELY: bot_move_succeed: path_found_rotate -> MOVE_ROTATING @@ -74,3 +84,19 @@ MOVE_WAIT_FOR_CLEAR_PATH: decrement counter. state_timeout: no_path_found_and_no_try_again -> MOVE_IDLE post failure. + +MOVE_LOADER_UNBLOCKING_UPING: + bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING + loader down + bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING + loader down + +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 + move backward + loader up + diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 46434b7f..a105e87d 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -47,8 +47,8 @@ Events: last command sent to the asserv board has been acquitted. import init.fsm -import move.fsm import loader.fsm +import move.fsm IDLE: start -> WAIT_INIT_TO_FINISH -- cgit v1.2.3