summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-13 00:22:32 +0200
committerNicolas Schodet2010-04-13 00:22:32 +0200
commit5cd1edab4210108f0753202deea45fd40b44efa5 (patch)
tree18477670bfe08e0b0e38128d3c364948f83bad9b /digital
parent621fd67190770e3f8bace929d5fb9f5633220475 (diff)
digital/io/src: update move FSM
Update branch names. Fix FSM lock up when no path found on FSM state. Fix FSM lock up when obstacle found and was going to final destination.
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_move_cb.c113
-rw-r--r--digital/io/src/move.fsm49
2 files changed, 72 insertions, 90 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index b52d8279..de68fef0 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -82,15 +82,11 @@ move_obstacle_in_table (vect_t pos)
* @return
* - 0 if no path could be found ;
* - 1 if a path has been found.
- * - 2 already at final place.
*/
uint8_t
move_get_next_position (void)
{
vect_t dst;
- /* Are we at the final position. */
- if (move_data.final_move)
- return 2;
/* Get the current position */
position_t current_pos;
asserv_get_position (&current_pos);
@@ -193,60 +189,61 @@ move_obstacle_here (void)
/*
* MOVE_IDLE =move_start=>
- * => MOVE_MOVING
- * ask the asserv to go to the computed position.
+ * path_found => MOVE_MOVING
+ * move to next position.
+ * no_path_found => MOVE_IDLE
+ * post failure event.
*/
fsm_branch_t
ai__MOVE_IDLE__move_start (void)
{
- /* ask the asserv to go to the computed position. */
- move_get_next_position ();
- return ai_next (MOVE_IDLE, move_start);
+ if (move_get_next_position ())
+ return ai_next_branch (MOVE_IDLE, move_start, path_found);
+ else
+ {
+ main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed;
+ return ai_next_branch (MOVE_IDLE, move_start, no_path_found);
+ }
}
/*
* MOVE_MOVING =bot_move_succeed=>
- * we_are_at_final_position => MOVE_IDLE
- * post an event for the top FSM to tell it we have finished.
- * position_intermediary => MOVE_MOVING
- * get next position computed by the path module.
- * if next position is the final, use a goto_xya.
- * otherwise go to the next intermediate position with goto.
- * no_intermediate_path_found => MOVE_IDLE
- * post an event for the top FSM to generate a failure.
+ * done => MOVE_IDLE
+ * post success event.
+ * path_found => MOVE_MOVING
+ * move to next position.
+ * no_path_found => MOVE_IDLE
+ * post failure event.
*/
fsm_branch_t
ai__MOVE_MOVING__bot_move_succeed (void)
{
- uint8_t ret = move_get_next_position ();
- if (ret == 2)
+ if (move_data.final_move)
{
- /* Post an event for the top FSM to tell it we have finished. */
main_post_event_for_top_fsm = AI_EVENT_move_fsm_succeed;
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, we_are_at_final_position);
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, done);
}
- else if (ret == 1)
+ else if (move_get_next_position ())
{
- /* Nothing to do. */
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, position_intermediary);
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found);
}
else
{
- /* Post an event for the top FSM to generate a failure. */
main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed;
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_intermediate_path_found);
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found);
}
}
/*
* MOVE_MOVING =bot_move_failed=>
* => MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- * compute the obstacle position.
+ * reset final_move.
* move backward to turn freely.
*/
fsm_branch_t
ai__MOVE_MOVING__bot_move_failed (void)
{
+ move_data.final_move = 0;
/* Compute the obstacle position. */
move_obstacle_here ();
/* Move backward to turn freely. */
@@ -258,97 +255,83 @@ ai__MOVE_MOVING__bot_move_failed (void)
/*
* MOVE_MOVING =obstacle_in_front=>
* => MOVE_WAIT_FOR_CLEAR_PATH
+ * reset final_move.
* stop the bot.
*/
fsm_branch_t
ai__MOVE_MOVING__obstacle_in_front (void)
{
- /* Stop the bot. */
+ move_data.final_move = 0;
asserv_stop_motor ();
return ai_next (MOVE_MOVING, obstacle_in_front);
}
/*
* MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
- * intermediate_path_found => MOVE_MOVING
- * get next intermediate position from path module.
- * no_intermediate_path_found => MOVE_IDLE
- * post an event for the top FSM to generate a failure.
+ * path_found => MOVE_MOVING
+ * move to next position.
+ * no_path_found => MOVE_IDLE
+ * post failure event.
*/
fsm_branch_t
ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
{
- uint8_t ret = move_get_next_position ();
- if (ret == 1)
+ if (move_get_next_position ())
{
- /* Nothing to do. */
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, intermediate_path_found);
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
}
else
{
- /* Post an event for the top FSM to generate a failure. */
main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed;
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_intermediate_path_found);
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_path_found);
}
}
/*
* MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
- * intermediate_path_found => MOVE_MOVING
- * get next intermediate position from path module
- * no_intermediate_path_found => MOVE_WAIT_FOR_CLEAR_PATH
+ * path_found => MOVE_MOVING
+ * move to next position.
+ * no_path_found => MOVE_WAIT_FOR_CLEAR_PATH
* nothing to do.
*/
fsm_branch_t
ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
{
- uint8_t ret = move_get_next_position ();
- if (ret == 1)
- {
- /* Nothing to do. */
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, intermediate_path_found);
- }
+ if (move_get_next_position ())
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
else
- {
- /* Nothing to do. */
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_intermediate_path_found);
- }
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found);
}
/*
* MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=>
- * no_more_obstacle_or_next_position => MOVE_MOVING
- * get next position computed by the path module.
- * if next position is the final, use a goto_xya.
- * otherwise go to the next intermediate position with goto.
- * obstacle_and_no_intermediate_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH
+ * path_found => MOVE_MOVING
+ * move to next position.
+ * no_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH
* decrement counter.
- * obstacle_and_no_intermediate_path_found_and_no_try_again => MOVE_IDLE
- * post an event for the top FSM to generate a failure.
+ * no_path_found_and_no_try_again => MOVE_IDLE
+ * post failure.
*/
fsm_branch_t
ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
{
- uint8_t ret = move_get_next_position ();
- if (ret == 1)
+ if (move_get_next_position ())
{
- /* Go to position. */
return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
- no_more_obstacle_or_next_position);
+ path_found);
}
else
{
/* Error, no new position, should we try again? */
if (--move_data.try_again_counter == 0)
{
- /* Post an event for the top FSM to generate a failure. */
main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed;
return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
- obstacle_and_no_intermediate_path_found_and_no_try_again);
+ no_path_found_and_no_try_again);
}
else
return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
- obstacle_and_no_intermediate_path_found_and_try_again);
+ no_path_found_and_try_again);
}
}
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index c1bfa9e9..d96cb349 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -22,40 +22,39 @@ Events:
the bot has seen something (front is the same when going backward).
MOVE_IDLE:
- move_start -> MOVE_MOVING
- ask the asserv to go to the computed position.
+ move_start: path_found -> MOVE_MOVING
+ move to next position.
+ move_start: no_path_found -> MOVE_IDLE
+ post failure event.
MOVE_MOVING:
- bot_move_succeed: we_are_at_final_position -> MOVE_IDLE
- post an event for the top FSM to tell it we have finished.
- bot_move_succeed: position_intermediary -> .
- get next position computed by the path module.
- if next position is the final, use a goto_xya.
- otherwise go to the next intermediate position with goto.
- bot_move_succeed: no_intermediate_path_found -> MOVE_IDLE
- post an event for the top FSM to generate a failure.
+ bot_move_succeed: done -> MOVE_IDLE
+ post success event.
+ bot_move_succeed: path_found -> .
+ move to next position.
+ bot_move_succeed: no_path_found -> MOVE_IDLE
+ post failure event.
bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
- compute the obstacle position.
+ reset final_move.
move backward to turn freely.
obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH
+ reset final_move.
stop the bot.
MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
- bot_move_succeed: intermediate_path_found -> MOVE_MOVING
- get next intermediate position from path module.
- bot_move_succeed: no_intermediate_path_found -> MOVE_IDLE
- post an event for the top FSM to generate a failure.
- bot_move_failed: intermediate_path_found -> MOVE_MOVING
- get next intermediate position from path module
- bot_move_failed: no_intermediate_path_found -> MOVE_WAIT_FOR_CLEAR_PATH
+ bot_move_succeed: path_found -> MOVE_MOVING
+ move to next position.
+ bot_move_succeed: no_path_found -> MOVE_IDLE
+ post failure event.
+ bot_move_failed: path_found -> MOVE_MOVING
+ move to next position.
+ bot_move_failed: no_path_found -> MOVE_WAIT_FOR_CLEAR_PATH
nothing to do.
MOVE_WAIT_FOR_CLEAR_PATH:
- state_timeout: no_more_obstacle_or_next_position -> MOVE_MOVING
- get next position computed by the path module.
- if next position is the final, use a goto_xya.
- otherwise go to the next intermediate position with goto.
- state_timeout: obstacle_and_no_intermediate_path_found_and_try_again -> .
+ state_timeout: path_found -> MOVE_MOVING
+ move to next position.
+ state_timeout: no_path_found_and_try_again -> .
decrement counter.
- state_timeout: obstacle_and_no_intermediate_path_found_and_no_try_again -> MOVE_IDLE
- post an event for the top FSM to generate a failure.
+ state_timeout: no_path_found_and_no_try_again -> MOVE_IDLE
+ post failure.