From 3051e493cb014bbcf7f375cf488a34b9b025a11e Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Thu, 21 May 2009 03:25:04 +0200 Subject: * digital/io/src: - rewrite move FSM. --- digital/io/src/main.c | 42 +---- digital/io/src/move.c | 3 +- digital/io/src/move.fsm | 129 +++++-------- digital/io/src/move.h | 6 +- digital/io/src/move_cb.c | 479 ++++++++++++----------------------------------- 5 files changed, 181 insertions(+), 478 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 20717222..06b5c3cb 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -84,13 +84,6 @@ uint8_t main_post_event_for_top_fsm = 0xFF; */ uint16_t main_sharp_ignore_event; -/** - * Flag for homologation, to disable the path finding and always stop in front - * of an obstacle and wait. - * Do not touch anymore ! XXX FIXME TODO - */ -uint8_t main_always_stop_for_obstacle = 1; - /** * Post an event for the main loop to wake up the move FSM in a certain count * of cycles. @@ -304,27 +297,18 @@ main_event_to_fsm (void) FSM_HANDLE_EVENT (&top_fsm, save_event); } /* Sharps event for move FSM */ - /* If we do not need to ignore sharp event */ - if (!main_sharp_ignore_event) + /* Get the current direction of the bot */ + uint8_t moving_direction = asserv_get_moving_direction (); + /* If we are moving */ + if (moving_direction) { - /* Get the current direction of the bot */ - uint8_t moving_direction = asserv_get_moving_direction (); - /* If we are moving */ - if (moving_direction) + if (sharp_path_obstrued (moving_direction)) { - if (sharp_path_obstrued (moving_direction)) - { - /* Generate an event for move FSM */ - FSM_HANDLE_EVENT (&move_fsm, - MOVE_EVENT_bot_move_obstacle); - } + /* Generate an event for move FSM */ + FSM_HANDLE_EVENT (&move_fsm, + MOVE_EVENT_obstacle_in_front); } } - /* Wait flag for move FSM */ - if (!main_move_wait_cycle) - { - FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_wait_finished); - } /* TODO: Check other sensors */ /* TODO: implement filterbridge events */ if(!IO_GET (CONTACT_FILTER_BRIDGE_PUCK)) @@ -804,16 +788,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) } } break; - case c ('o', 1): - { - /* Omo-logo-ation flag, to prevent avoiding obstacle and stop - * instead. - * - 1b: state of the flag (0 to disable, 1 to enable). - * Do not touch anymore ! XXX FIXME TODO - */ - main_always_stop_for_obstacle = args[0]; - } - break; default: { uint8_t error = 1; diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 28dcfeac..181e4545 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -42,8 +42,7 @@ move_start (asserv_position_t position, uint8_t backward) move_data.final.y = position.y; move_data.final.a = position.a; move_data.backward_movement_allowed = backward; - /* XXX */ - main_always_stop_for_obstacle = 1; + move_data.final_move = 0; /* Reset move FSM flags */ main_sharp_ignore_event = 0; main_move_wait_cycle = 0; diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index f5a56253..50f104e2 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -3,104 +3,65 @@ # desired one. It will try to avoid obstacle and manage the case when the # asserv is blocked. move + move with avoid obstacle. States: IDLE - waiting for the beginning of the move FSM - MOVING_TO_FINAL_POSITION - moving to the final position - MOVING_BACKWARD - moving backward to go away from what is blocking the bot - MOVING_TO_INTERMEDIATE_POSITION - moving to an intermediate position to try avoiding the obstacle - WAIT_FOR_CLEAR_PATH - waiting for obstacle to disapear + waiting for the beginning of the move FSM. + MOVING + moving to a position (intermediate or final). + MOVING_BACKWARD_TO_TURN_FREELY + moving backward to go away from what is blocking the bot. + WAIT_FOR_CLEAR_PATH [timeout=255] + waiting for obstacle to disappear. Events: start - initialize the FSM + initialize the FSM and start the movement directly. bot_move_failed - the bot movement failed (blocked by something for example) + the bot movement failed (blocked by something for example). bot_move_succeed - the bot has finished to moved and arrived at the desired position - bot_move_obstacle - the bot has seen something (with the sharps) - wait_finished - we have wait the desired time + the bot has finished to moved and arrived at the desired position. + obstacle_in_front + the bot has seen something (with the sharps) (front is the same when going backward). state_timeout + timeout. IDLE: - start: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - ask the asserv to go to the intermediate position - start: no_intermediate_path_found -> MOVING_TO_FINAL_POSITION - ask the asserv to go to the final position + start -> MOVING + ask the asserv to go to the computed position. -MOVING_TO_FINAL_POSITION: - bot_move_succeed -> IDLE - post an event for the top FSM to tell we have finished - bot_move_failed -> MOVING_BACKWARD - compute the obstacle position - store current moving direction (for possible failed of path module) - move backward to turn freely - bot_move_obstacle: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - compute the obstacle position - get next intermediate position from path module - go to next intermediate position - bot_move_obstacle: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - compute the obstacle position - get next intermediate position from path module failed - store current moving direction - stop the bot - post an event for the top FSM to be waked up later +MOVING: + bot_move_succeed: we_are_at_final_position -> 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 -> IDLE + post an event for the top FSM to generate a failure. + bot_move_failed -> MOVING_BACKWARD_TO_TURN_FREELY + compute the obstacle position. + move backward to turn freely. + obstacle_in_front -> WAIT_FOR_CLEAR_PATH + stop the bot. -MOVING_BACKWARD: -# TODO -# We ignore obstacle when moving backward - bot_move_succeed: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - get next intermediate position from path module - bot_move_succeed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - get next intermediate position from path module, failed - stop the bot - post an event for the top FSM to be waked up later -# Do the same as when we succeed - bot_move_failed: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION +MOVING_BACKWARD_TO_TURN_FREELY: + bot_move_succeed: intermediate_path_found -> MOVING + get next intermediate position from path module. + bot_move_succeed: no_intermediate_path_found -> IDLE + post an event for the top FSM to generate a failure. + bot_move_failed: intermediate_path_found -> MOVING get next intermediate position from path module bot_move_failed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - get next intermediate position from path module, failed - stop the bot - post an event for the top FSM to be waked up later - -MOVING_TO_INTERMEDIATE_POSITION: - bot_move_obstacle: intermediate_path_found -> . - compute the obstacle position - get next intermediate position from path module - go to next intermediate position - bot_move_obstacle: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - compute the obstacle position - get next intermediate position from path module failed - store current moving direction - stop the bot - post an event for the top FSM to be waked up later - bot_move_succeed: final_position -> IDLE - post an event for the top FSM to tell we have finished - bot_move_succeed: position_intermediary -> . - go to the next intermediate position computed by the path module - bot_move_succeed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH - store current moving direction - stop the bot - post an event for the top FSM to be waked up later - bot_move_failed -> MOVING_BACKWARD - store the current position of the obstacle - move backward to turn freely + nothing to do. WAIT_FOR_CLEAR_PATH: - wait_finished: no_obstacle -> MOVING_TO_FINAL_POSITION - ask the asserv to go to the final position - wait_finished: obstacle_and_intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION - compute the obstacle position - get next intermediate position from path module - go to next intermediate position - wait_finished: obstacle_and_no_intermediate_path_found -> . - compute the obstacle position - get next intermediate position from path module failed - post an event for the top FSM to be waked up later + state_timeout: no_more_obstacle_or_next_position -> 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 -> . + decrement counter. + state_timeout: obstacle_and_no_intermediate_path_found_and_no_try_again -> IDLE + post an event for the top FSM to generate a failure. diff --git a/digital/io/src/move.h b/digital/io/src/move.h index 020bc2f4..48d7a3ea 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -53,8 +53,10 @@ struct move_data_t move_position_t obstacle; /** Backward direction allowed flag. */ uint8_t backward_movement_allowed; - /** Cached moving direction of the bot when blocked. */ - uint8_t cached_moving_direction; + /** Try again counter. */ + uint8_t try_again_counter; + /** Dirty fix to know this is the final move. */ + uint8_t final_move; }; /** diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 95dc3df9..03b32731 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -30,10 +30,12 @@ #include "playground.h" #include "move.h" #include "sharp.h" +#include "aquajim.h" #include "main.h" /* main_post_event_for_top_fsm */ #include "modules/math/fixed/fixed.h" /* fixed_* */ #include "modules/path/path.h" +#include "modules/utils/utils.h" #include "debug.host.h" @@ -109,10 +111,14 @@ move_obstacle_in_table (move_position_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 (move_position_t *dst) { + /* Are we at the final position. */ + if (move_data.final_move) + return 2; /* Get the current position */ asserv_position_t current_pos; asserv_get_position (¤t_pos); @@ -125,22 +131,31 @@ move_get_next_position (move_position_t *dst) /* If the path is found. */ if (path_get_next (&dst->x, &dst->y) != 0) { - if ((dst->x != move_data.final.x || dst->y != move_data.final.y) - && main_always_stop_for_obstacle) + /* If it is not the last position. */ + if (dst->x != move_data.final.x || dst->y != move_data.final.y) { - DPRINTF ("Ignore alternative path !\n"); - return 0; + /* Not final position. */ + move_data.final_move = 0; + /* Goto without angle. */ + asserv_goto (dst->x, dst->y, + move_data.backward_movement_allowed); } else { - main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; - DPRINTF ("Computed path is (%d ; %d)\n", dst->x, dst->y); - return 1; + /* Final position. */ + move_data.final_move = 1; + /* Goto with angle. */ + asserv_goto_xya (dst->x, dst->y, move_data.final.a, + move_data.backward_movement_allowed); } + /* Reset try counter. */ + move_data.try_again_counter = 3; + return 1; } else { - DPRINTF ("Could not compute any path to avoid obstacle!\n"); + /* Error, not final move. */ + move_data.final_move = 0; return 0; } } @@ -154,44 +169,18 @@ void move_compute_obstacle_position (asserv_position_t cur, move_position_t *obstacle) { - uint8_t i; - uint16_t dist; + int16_t dist; /* Convert the angle */ uint32_t angle = cur.a; - /* Change angle when going backward */ - if (0) - angle += 0x8000; angle = angle << 8; - DPRINTF ("We are at (%d ; %d ; %x)\n", cur.x, cur.y, cur.a); - // Forward moving. - if (asserv_get_moving_direction () == 1) + /* Dirty fix: distance of the obstacle. */ + dist = BOT_LENGTH / 2 + 350; + /* Invert if last movement was backward. */ + if (asserv_get_last_moving_direction () == 2) { - uint16_t dist_computed; - dist = 0xFFFF; - for (i = 0; i < 3; i ++) - { - if (dist > - (dist_computed = sharp_get_distance_mm (sharp_get_raw(i)))) - { - dist = dist_computed; - } - } - } - // Backward moving. - else - { - uint16_t dist_computed; - dist = 0xFFFF; - for (i = 3; i < 5; i ++) - { - if (dist > - (dist_computed = sharp_get_distance_mm (sharp_get_raw(i)))) - { - dist = dist_computed; - } - } + dist = -dist; } /* X */ @@ -200,7 +189,6 @@ move_compute_obstacle_position (asserv_position_t cur, /* Y */ obstacle->y = cur.y + fixed_mul_f824 (fixed_sin_f824 (angle), dist); - DPRINTF ("Computed obstacle (%d ; %d)\n", obstacle->x, obstacle->y); } /** @@ -225,393 +213,172 @@ move_obstacle_here (void) } else { - main_sharp_ignore_event = 0xFF; DPRINTF ("Obstacle Ignored pos x : %d, pos y : %d\n", move_data.obstacle.x, move_data.obstacle.y); } } -/** - * Unique function after moving backward to have unique code. - * @return the value of the move_get_next_position. - */ -uint8_t -move_after_moving_backward (void) -{ - /* Give the current position of the bot to the path module */ - if (move_get_next_position (&move_data.intermediate)) - { - /* Go to the next position */ - if (move_data.intermediate.x == move_data.final.x - && move_data.intermediate.y == move_data.final.y) - asserv_goto_xya (move_data.intermediate.x, - move_data.intermediate.y, - move_data.final.a, - move_data.backward_movement_allowed); - else - asserv_goto (move_data.intermediate.x, - move_data.intermediate.y, - move_data.backward_movement_allowed); - - - return 1; - } - else - return 0; -} - /* * IDLE =start=> - * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION - * ask the asserv to go to the intermediate position - * no_intermediate_path_found => MOVING_TO_FINAL_POSITION - * ask the asserv to go to the final position + * => MOVING + * ask the asserv to go to the computed position. */ fsm_branch_t move__IDLE__start (void) { - if (!move_get_next_position (&move_data.intermediate)) - { - if (move_data.intermediate.x == move_data.final.x - && move_data.intermediate.y == move_data.final.y) - { - /* Go to the destination position */ - asserv_goto_xya (move_data.final.x, - move_data.final.y, - move_data.final.a, - move_data.backward_movement_allowed); - return move_next_branch (IDLE, start, no_intermediate_path_found); - } - else - { - asserv_goto (move_data.intermediate.x, - move_data.intermediate.y, - move_data.backward_movement_allowed); - return move_next_branch (IDLE, start, intermediate_path_found); - } - } - else - { - asserv_goto_xya (move_data.final.x, - move_data.final.y, - move_data.final.a, - move_data.backward_movement_allowed); - return move_next_branch (IDLE, start, no_intermediate_path_found); - } + /* ask the asserv to go to the computed position. */ + move_get_next_position (&move_data.intermediate); + return move_next (IDLE, start); } /* - * WAIT_FOR_CLEAR_PATH =wait_finished=> - * no_obstacle => MOVING_TO_FINAL_POSITION - * ask the asserv to go to the final position - * obstacle_and_no_intermediate_path_found => WAIT_FOR_CLEAR_PATH - * compute the obstacle position - * get next intermediate position from path module failed - * post an event for the top FSM to be waked up later - * obstacle_and_intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION - * compute the obstacle position - * get next intermediate position from path module - * go to next intermediate position + * MOVING =bot_move_succeed=> + * we_are_at_final_position => IDLE + * post an event for the top FSM to tell it we have finished. + * position_intermediary => 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 => IDLE + * post an event for the top FSM to generate a failure. */ fsm_branch_t -move__WAIT_FOR_CLEAR_PATH__wait_finished (void) +move__MOVING__bot_move_succeed (void) { -/// if (!sharp_path_obstrued (move_data.cached_moving_direction)) - if (!sharp_path_obstrued (1)) + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 2) { - /* Try to go to the final position */ - asserv_goto_xya (move_data.final.x, move_data.final.y, - move_data.final.a, - move_data.backward_movement_allowed); - return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, no_obstacle); - } - else - { - /* Enable path finding */ - main_always_stop_for_obstacle = 0; - /* Get next position */ - if (path_get_next (&move_data.intermediate.x, &move_data.intermediate.y)) - { - /* Ignore sharps */ - main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; - if (move_data.intermediate.x == move_data.final.x - && move_data.intermediate.y == move_data.final.y) - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, - move_data.intermediate.y, - move_data.final.a, - move_data.backward_movement_allowed); - else - asserv_goto (move_data.intermediate.x, - move_data.intermediate.y, - move_data.backward_movement_allowed); - - return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found); - } - else - { - /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_moving_direction (); - /* Stop the bot */ - asserv_stop_motor (); - /* Post an event for the top FSM to be waked up later */ - main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP; - return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_no_intermediate_path_found); - } + /* Post an event for the top FSM to tell it we have finished. */ + main_post_event_for_top_fsm = TOP_EVENT_move_fsm_succeed; + return move_next_branch (MOVING, bot_move_succeed, we_are_at_final_position); } -} - - -/* - * MOVING_TO_INTERMEDIATE_POSITION =bot_move_succeed=> - * final_position => IDLE - * post an event for the top FSM to tell we have finished - * position_intermediary => MOVING_TO_INTERMEDIATE_POSITION - * go to the next intermediate position computed by the path module - * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH - * store current moving direction - * stop the bot - * post an event for the top FSM to be waked up later - */ -fsm_branch_t -move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void) -{ - if ((move_data.final.x == move_data.intermediate.x) && - (move_data.final.y == move_data.intermediate.y)) + else if (ret == 1) { - /* Post an event for the top FSM to tell we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_move_fsm_succeed; - return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, final_position); + /* Nothing to do. */ + return move_next_branch (MOVING, bot_move_succeed, position_intermediary); } else { - /* Get next position */ - if (move_get_next_position (&move_data.intermediate)) - { - if (move_data.intermediate.x == move_data.final.x - && move_data.intermediate.y == move_data.final.y) - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, - move_data.intermediate.y, - move_data.final.a, - move_data.backward_movement_allowed); - else - asserv_goto (move_data.intermediate.x, - move_data.intermediate.y, - move_data.backward_movement_allowed); - - return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary); - } - else - { - /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_moving_direction (); - /* Stop the bot */ - asserv_stop_motor (); - /* Post an event for the top FSM to be waked up later */ - main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP; - return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, no_intermediate_path_found); - } + /* Post an event for the top FSM to generate a failure. */ + main_post_event_for_top_fsm = TOP_EVENT_move_fsm_failed; + return move_next_branch (MOVING, bot_move_succeed, no_intermediate_path_found); } } + /* - * MOVING_TO_INTERMEDIATE_POSITION =bot_move_obstacle=> - * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION - * compute the obstacle position - * get next intermediate position from path module - * go to next intermediate position - * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH - * compute the obstacle position - * get next intermediate position from path module failed - * store current moving direction - * stop the bot - * post an event for the top FSM to be waked up later + * MOVING =bot_move_failed=> + * => MOVING_BACKWARD_TO_TURN_FREELY + * compute the obstacle position. + * move backward to turn freely. */ fsm_branch_t -move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void) +move__MOVING__bot_move_failed (void) { - /* Compute obstacle position */ + /* Compute the obstacle position. */ move_obstacle_here (); - /* Get next position */ - if (move_get_next_position (&move_data.intermediate)) - { - if (move_data.intermediate.x == move_data.final.x - && move_data.intermediate.y == move_data.final.y) - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, - move_data.intermediate.y, - move_data.final.a, - move_data.backward_movement_allowed); - else - asserv_goto (move_data.intermediate.x, - move_data.intermediate.y, - move_data.backward_movement_allowed); - return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, intermediate_path_found); - } - else - { - /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_moving_direction (); - /* Stop the bot */ - asserv_stop_motor (); - /* Post an event for the top FSM to be waked up later */ - main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP; - return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, no_intermediate_path_found); - } + /* Move backward to turn freely. */ + asserv_move_linearly (asserv_get_last_moving_direction () == 1 ? + - 300 : 300); + return move_next (MOVING, bot_move_failed); } /* - * MOVING_TO_INTERMEDIATE_POSITION =bot_move_failed=> - * => MOVING_BACKWARD - * store the current position of the obstacle - * move backward to turn freely + * MOVING =obstacle_in_front=> + * => WAIT_FOR_CLEAR_PATH + * stop the bot. */ fsm_branch_t -move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_failed (void) +move__MOVING__obstacle_in_front (void) { - /* Compute obstacle position */ - move_obstacle_here (); - /* Go backward */ - asserv_move_linearly (-PG_MOVE_DISTANCE); - return move_next (MOVING_TO_INTERMEDIATE_POSITION, bot_move_failed); + /* Stop the bot. */ + asserv_stop_motor (); + return move_next (MOVING, obstacle_in_front); } /* - * MOVING_BACKWARD =bot_move_failed=> - * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION - * get next intermediate position from path module - * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH - * get next intermediate position from path module, failed - * stop the bot - * post an event for the top FSM to be waked up later + * MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=> + * intermediate_path_found => MOVING + * get next intermediate position from path module. + * no_intermediate_path_found => IDLE + * post an event for the top FSM to generate a failure. */ fsm_branch_t -move__MOVING_BACKWARD__bot_move_failed (void) +move__MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) { - /* Call generic function */ - if (move_after_moving_backward ()) + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 1) { - return move_next_branch (MOVING_BACKWARD, bot_move_failed, intermediate_path_found); + /* Nothing to do. */ + return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, intermediate_path_found); } else { - /* Store current moving direction */ - move_data.cached_moving_direction = 2; - /* Stop the bot */ - asserv_stop_motor (); - /* Post an event for the top FSM to be waked up later */ - main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP; - return move_next_branch (MOVING_BACKWARD, bot_move_failed, no_intermediate_path_found); + /* Post an event for the top FSM to generate a failure. */ + main_post_event_for_top_fsm = TOP_EVENT_move_fsm_failed; + return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_intermediate_path_found); } } /* - * MOVING_BACKWARD =bot_move_succeed=> - * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION + * MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=> + * intermediate_path_found => MOVING * get next intermediate position from path module * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH - * get next intermediate position from path module, failed - * stop the bot - * post an event for the top FSM to be waked up later + * nothing to do. */ fsm_branch_t -move__MOVING_BACKWARD__bot_move_succeed (void) +move__MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) { - /* Call generic function */ - if (move_after_moving_backward ()) + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 1) { - return move_next_branch (MOVING_BACKWARD, bot_move_succeed, intermediate_path_found); + /* Nothing to do. */ + return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, intermediate_path_found); } else { - /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_moving_direction (); - /* Stop the bot */ - asserv_stop_motor (); - /* Post an event for the top FSM to be waked up later */ - main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP; - return move_next_branch (MOVING_BACKWARD, bot_move_succeed, no_intermediate_path_found); + /* Nothing to do. */ + return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_intermediate_path_found); } } /* - * MOVING_TO_FINAL_POSITION =bot_move_failed=> - * => MOVING_BACKWARD - * compute the obstacle position - * store current moving direction (for possible failed of path module) - * move backward to turn freely - */ -fsm_branch_t -move__MOVING_TO_FINAL_POSITION__bot_move_failed (void) -{ - /* Compute obstacle position */ - move_obstacle_here (); - /* Store current moving direction */ - move_data.cached_moving_direction = move_data.backward_movement_allowed ? - 2 : 1; - /* Move backward to turn freely */ - asserv_move_linearly (-PG_MOVE_DISTANCE); - return move_next (MOVING_TO_FINAL_POSITION, bot_move_failed); -} - -/* - * MOVING_TO_FINAL_POSITION =bot_move_succeed=> - * => IDLE - * post an event for the top FSM to tell we have finished - */ -fsm_branch_t -move__MOVING_TO_FINAL_POSITION__bot_move_succeed (void) -{ - /* Post an event for the top FSM to tell we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_move_fsm_succeed; - return move_next (MOVING_TO_FINAL_POSITION, bot_move_succeed); -} - -/* - * MOVING_TO_FINAL_POSITION =bot_move_obstacle=> - * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION - * compute the obstacle position - * get next intermediate position from path module - * go to next intermediate position - * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH - * compute the obstacle position - * get next intermediate position from path module failed - * store current moving direction - * stop the bot - * post an event for the top FSM to be waked up later + * WAIT_FOR_CLEAR_PATH =state_timeout=> + * no_more_obstacle_or_next_position => 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 => WAIT_FOR_CLEAR_PATH + * decrement counter. + * obstacle_and_no_intermediate_path_found_and_no_try_again => IDLE + * post an event for the top FSM to generate a failure. */ fsm_branch_t -move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void) +move__WAIT_FOR_CLEAR_PATH__state_timeout (void) { - /* Compute obstacle position */ - move_obstacle_here (); - /* Get next position */ - if (move_get_next_position (&move_data.intermediate)) + if (sharp_path_obstrued (asserv_get_last_moving_direction ())) + move_obstacle_here (); + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 1) { - if (move_data.intermediate.x == move_data.final.x - && move_data.intermediate.y == move_data.final.y) - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, - move_data.intermediate.y, - move_data.final.a, - move_data.backward_movement_allowed); - else - asserv_goto (move_data.intermediate.x, - move_data.intermediate.y, - move_data.backward_movement_allowed); - return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, intermediate_path_found); + /* Go to position. */ + return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout, + no_more_obstacle_or_next_position); } else { - /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_moving_direction (); - /* Stop the bot */ - asserv_stop_motor (); - /* Post an event for the top FSM to be waked up later */ - main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP; - return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, no_intermediate_path_found); + /* Error, no new position, should we try again? */ + if (--move_data.try_again_counter == 0) + { + return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout, + obstacle_and_no_intermediate_path_found_and_no_try_again); + } + else + return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout, + obstacle_and_no_intermediate_path_found_and_try_again); } } + -- cgit v1.2.3