From ed51ec3c5eb23aecb3a93b02814acf655ff62a0c Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Thu, 1 May 2008 01:08:56 +0200 Subject: * digital/io/src - enhance move FSM with support to stop when there is no valid path found to avoid the obstacle ; - add a flag to be notified from the main loop after a certain amount of time. --- digital/io/src/main.c | 14 +++ digital/io/src/main.h | 8 ++ digital/io/src/move.fsm | 66 ++++++++++---- digital/io/src/move.h | 2 + digital/io/src/move_cb.c | 219 ++++++++++++++++++++++++++++++++++++----------- 5 files changed, 243 insertions(+), 66 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/main.c b/digital/io/src/main.c index e5d25cf3..b40eefd5 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -75,6 +75,12 @@ uint8_t main_post_event_for_top_fsm = 0xFF; */ uint16_t main_sharp_ignore_event; +/** + * Post an event for the main loop to wake up the move FSM in a certain count + * of cycles. + */ +uint16_t main_move_wait_cycle; + /** * Sharps stats counters. */ @@ -182,6 +188,9 @@ main_loop (void) /* Update the ignore sharp event flag */ if (main_sharp_ignore_event) main_sharp_ignore_event--; + /* Update wait flag for move FSM */ + if (main_move_wait_cycle) + main_move_wait_cycle--; /* Update sharp module if required and only every * MAIN_SHARP_UPDATE_FREQ cycles */ if (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ) @@ -282,6 +291,11 @@ main_loop (void) } } } + /* Wait flag for move FSM */ + if (!main_move_wait_cycle) + { + FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_wait_finished); + } /* TODO: Check other sensors */ } diff --git a/digital/io/src/main.h b/digital/io/src/main.h index 24359c6d..96efb10a 100644 --- a/digital/io/src/main.h +++ b/digital/io/src/main.h @@ -40,4 +40,12 @@ extern uint8_t main_post_event_for_top_fsm; */ extern uint16_t main_sharp_ignore_event; +/** + * Post an event for the main loop to wake up the move FSM in a certain count + * of cycles. + * This flag is used by the move FSM to wait a certain amount of time before + * checking on the sharps again. + */ +extern uint16_t main_move_wait_cycle; + #endif /* main_h */ diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index ab55d1d4..317f25d8 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -13,6 +13,8 @@ States: 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 Events: start @@ -23,6 +25,8 @@ Events: 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 IDLE: start -> MOVING_TO_FINAL_POSITION @@ -32,36 +36,64 @@ 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 - store the current position of the obstacle + compute the obstacle position + store current moving direction (for possible failed of path module) move backward to turn freely - bot_move_obstacle -> 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 + bot_move_obstacle: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH compute the obstacle position - give the obstacle position to the path module - give the current position of the bot to the path module - get the intermediate position from path module + 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_BACKWARD: # TODO # We ignore obstacle when moving backward - bot_move_succeed -> MOVING_TO_INTERMEDIATE_POSITION - give the obstacle position to the path module - give the current position of the bot to the path module - get the intermediate position from path module - bot_move_failed -> MOVING_TO_INTERMEDIATE_POSITION - do the same as when we succeed - give the obstacle position to the path module - give the current position of the bot to the path module - get the intermediate position from path module + 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 + 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 -> . + bot_move_obstacle: intermediate_path_found -> . compute the obstacle position - give the obstacle position to the path module - go to the next intermediate position computed by the path module + 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 + +WAIT_FOR_CLEAR_PATH: + wait_finished: obstacle -> . + check for obstacle using stored moving direction + post an event for the top FSM to be waked up later + wait_finished: no_obstacle -> MOVING_TO_FINAL_POSITION + check for obstacle using stored moving direction + try to go the final position diff --git a/digital/io/src/move.h b/digital/io/src/move.h index 5a328950..1b1621af 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -49,6 +49,8 @@ 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; }; /** diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 5b1c63a6..8393c60d 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -30,6 +30,7 @@ #include "asserv.h" #include "playground.h" #include "move.h" +#include "sharp.h" #include "main.h" /* main_post_event_for_top_fsm */ #include "modules/math/fixed/fixed.h" /* fixed_* */ @@ -74,11 +75,20 @@ */ #define MOVE_MAIN_IGNORE_SHARP_EVENT (3 * 225) +/** + * Number of cycles to wait before trying to read the sharps values again when + * we are stopped. + */ +#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (50) + /** * Easier function to get the next intermediate positon from the path module. * @param dst new destination position computed by the path module + * @return + * - 0 if no path could be found ; + * - 1 if a path has been found. */ -void +uint8_t move_get_next_position (move_position_t *dst) { /* Get the current position */ @@ -94,12 +104,11 @@ move_get_next_position (move_position_t *dst) if (!path_get_next (&dst->x, &dst->y)) { DPRINTF ("Could not compute any path to avoid obstacle!\n"); - /* If it failed, try original destination */ - dst->x = move_data.final.x; - dst->y = move_data.final.y; + return 0; } main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; DPRINTF ("Computed path is (%d ; %d)\n", dst->x, dst->y); + return 1; } /** @@ -144,17 +153,52 @@ move_obstacle_here (void) /** * Unique function after moving backward to have unique code. + * @return the value of the move_get_next_position. */ -void +uint8_t move_after_moving_backward (void) { /* Give the current position of the bot to the path module */ - move_get_next_position (&move_data.intermediate); - /* Go to the next position */ - if (move_data.backward_movement_allowed) - asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + if (move_get_next_position (&move_data.intermediate)) + { + /* Go to the next position */ + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return 1; + } + else + return 0; +} + +/* + * WAIT_FOR_CLEAR_PATH =wait_finished=> + * no_obstacle => MOVING_TO_FINAL_POSITION + * check for obstacle using stored moving direction + * try to go the final position + * obstacle => WAIT_FOR_CLEAR_PATH + * check for obstacle using stored moving direction + * post an event for the top FSM to be waked up later + */ +fsm_branch_t +move__WAIT_FOR_CLEAR_PATH__wait_finished (void) +{ + if (!sharp_path_obstrued (move_data.cached_moving_direction)) + { + /* Try to go to the final position */ + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.final.x, move_data.final.y); + else + asserv_goto (move_data.final.x, move_data.final.y); + return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, no_obstacle); + } else - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + { + /* 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); + } } /* @@ -179,6 +223,10 @@ move__IDLE__start (void) * 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) @@ -193,22 +241,40 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void) else { /* Get next position */ - move_get_next_position (&move_data.intermediate); - /* Go to the next intermediate position */ - if (move_data.backward_movement_allowed) - asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + if (move_get_next_position (&move_data.intermediate)) + { + /* Go to the next intermediate position */ + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary); + } else - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); - return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary); + { + /* 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); + } } } /* * MOVING_TO_INTERMEDIATE_POSITION =bot_move_obstacle=> - * => MOVING_TO_INTERMEDIATE_POSITION + * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION * compute the obstacle position - * give the obstacle position to the path module - * go to the next intermediate position computed by the path module + * 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 */ fsm_branch_t move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void) @@ -216,13 +282,25 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void) /* Compute obstacle position */ move_obstacle_here (); /* Get next position */ - move_get_next_position (&move_data.intermediate); - /* Go to the next intermediate position */ - if (move_data.backward_movement_allowed) - asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + if (move_get_next_position (&move_data.intermediate)) + { + /* Go to the next intermediate position */ + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, intermediate_path_found); + } else - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); - return move_next (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle); + { + /* 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); + } } /* @@ -243,39 +321,63 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_failed (void) /* * MOVING_BACKWARD =bot_move_failed=> - * => MOVING_TO_INTERMEDIATE_POSITION - * do the same as when we succeed - * give the obstacle position to the path module - * give the current position of the bot to the path module - * get the intermediate position from path module + * 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 */ fsm_branch_t move__MOVING_BACKWARD__bot_move_failed (void) { /* Call generic function */ - move_after_moving_backward (); - return move_next (MOVING_BACKWARD, bot_move_failed); + if (move_after_moving_backward ()) + { + return move_next_branch (MOVING_BACKWARD, bot_move_failed, intermediate_path_found); + } + else + { + /* 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); + } } /* * MOVING_BACKWARD =bot_move_succeed=> - * => MOVING_TO_INTERMEDIATE_POSITION - * give the obstacle position to the path module - * give the current position of the bot to the path module - * get the intermediate position from path module + * 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 */ fsm_branch_t move__MOVING_BACKWARD__bot_move_succeed (void) { /* Call generic function */ - move_after_moving_backward (); - return move_next (MOVING_BACKWARD, bot_move_succeed); + if (move_after_moving_backward ()) + { + return move_next_branch (MOVING_BACKWARD, bot_move_succeed, intermediate_path_found); + } + else + { + /* 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); + } } /* * MOVING_TO_FINAL_POSITION =bot_move_failed=> * => MOVING_BACKWARD - * store the current position of the obstacle + * compute the obstacle position + * store current moving direction (for possible failed of path module) * move backward to turn freely */ fsm_branch_t @@ -283,6 +385,8 @@ move__MOVING_TO_FINAL_POSITION__bot_move_failed (void) { /* Compute obstacle position */ move_obstacle_here (); + /* Store current moving direction */ + move_data.cached_moving_direction = asserv_get_moving_direction (); /* Move backward to turn freely */ asserv_move_linearly (-PG_MOVE_DISTANCE); return move_next (MOVING_TO_FINAL_POSITION, bot_move_failed); @@ -303,11 +407,16 @@ move__MOVING_TO_FINAL_POSITION__bot_move_succeed (void) /* * MOVING_TO_FINAL_POSITION =bot_move_obstacle=> - * => MOVING_TO_INTERMEDIATE_POSITION + * 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 - * give the obstacle position to the path module - * give the current position of the bot to the path module - * get the intermediate position from path module + * 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 */ fsm_branch_t move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void) @@ -315,12 +424,24 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void) /* Compute obstacle position */ move_obstacle_here (); /* Get next position */ - move_get_next_position (&move_data.intermediate); - /* Go to the next intermediate position */ - if (move_data.backward_movement_allowed) - asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + if (move_get_next_position (&move_data.intermediate)) + { + /* Go to the next intermediate position */ + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, intermediate_path_found); + } else - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); - return move_next (MOVING_TO_FINAL_POSITION, bot_move_obstacle); + { + /* 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); + } } -- cgit v1.2.3