From f51b15ddb7c1045e0edf8b50bb6933db7fcf19f3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 29 May 2010 13:44:53 +0200 Subject: digital/io/src: add more blocking handling and timeout --- digital/io/src/ai_move_cb.c | 37 +++++++++++++++++++++++++++++++++++++ digital/io/src/move.fsm | 11 +++++++++-- 2 files changed, 46 insertions(+), 2 deletions(-) (limited to 'digital') diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index 5fc70653..69a46a3c 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -247,6 +247,30 @@ ai__MOVE_ROTATING__bot_move_succeed (void) return ai_next (MOVE_ROTATING, bot_move_succeed); } +/* + * MOVE_ROTATING =bot_move_failed=> + * => MOVE_MOVING + * move to next position. + */ +fsm_branch_t +ai__MOVE_ROTATING__bot_move_failed (void) +{ + ai__MOVE_ROTATING__bot_move_succeed (); + return ai_next (MOVE_ROTATING, bot_move_failed); +} + +/* + * MOVE_ROTATING =state_timeout=> + * => MOVE_MOVING + * move to next position. + */ +fsm_branch_t +ai__MOVE_ROTATING__state_timeout (void) +{ + ai__MOVE_ROTATING__bot_move_succeed (); + return ai_next (MOVE_ROTATING, state_timeout); +} + /* * MOVE_ROTATING =loader_errored=> * => MOVE_LOADER_UNBLOCKING_UPING @@ -318,6 +342,19 @@ ai__MOVE_MOVING__bot_move_failed (void) return ai_next (MOVE_MOVING, bot_move_failed); } +/* + * MOVE_MOVING =state_timeout=> + * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY + * reset final_move. + * move backward to turn freely. + */ +fsm_branch_t +ai__MOVE_MOVING__state_timeout (void) +{ + ai__MOVE_MOVING__bot_move_failed (); + return ai_next (MOVE_MOVING, state_timeout); +} + /* * MOVE_MOVING =obstacle_in_front=> * tryagain => MOVE_WAIT_FOR_CLEAR_PATH diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index fad6b6b7..05f0c9d0 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -8,9 +8,9 @@ move States: *MOVE_IDLE waiting for the beginning of the move FSM. - MOVE_ROTATING + MOVE_ROTATING [timeout=1125] rotating towards next point. - MOVE_MOVING + MOVE_MOVING [timeout=2250] moving to a position (intermediate or final). MOVE_MOVING_BACKWARD_TO_TURN_FREELY moving backward to go away from what is blocking the bot. @@ -38,6 +38,10 @@ MOVE_IDLE: MOVE_ROTATING: bot_move_succeed -> MOVE_MOVING move to next position. + bot_move_failed -> MOVE_MOVING + move to next position. + state_timeout -> MOVE_MOVING + move to next position. loader_errored -> MOVE_LOADER_UNBLOCKING_UPING move backward loader up @@ -54,6 +58,9 @@ MOVE_MOVING: bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY reset final_move. move backward to turn freely. + state_timeout -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY + reset final_move. + move backward to turn freely. obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH reset final_move. stop the bot. -- cgit v1.2.3