From 1bcff7dbcbb284f5564ac708292b23fd447395a9 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Sat, 3 May 2008 11:15:31 +0200 Subject: * digital/io/src * top FSM - change the position where to drop balls at the gutter (to prevent the order of dropped balls to change) ; - change the order of balls taken at the distributor: first two samples with three ice, then three samples with two ice. * get samples FSM - manage the case where the arm passed the noted position in the state where it should not (because the arm was blocked) ; - add some debug messages. * gutter FSM - reduce the time spent to drop the balls at the gutter. * move FSM - update the algorithm to avoid obstacle: when a obstacle is seen, first way, then if it is not present anymore continue, otherwise, try to find an alternative path; if none found, wait again. * chrono - near end of the match time increased to 83s. --- digital/io/src/move_cb.c | 43 ++++++++++++++++++++++++++++++++++--------- 1 file changed, 34 insertions(+), 9 deletions(-) (limited to 'digital/io/src/move_cb.c') diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 0de7ea41..e85a4ef7 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -68,7 +68,7 @@ /** * The generic validity time (in term of number of cyles). */ -#define MOVE_OBSTACLE_VALIDITY (4 * 225) +#define MOVE_OBSTACLE_VALIDITY (6 * 225) /** * Cycles count to ignore sharp event in the main loop. @@ -79,7 +79,7 @@ * 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) +#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (MOVE_MAIN_IGNORE_SHARP_EVENT) /** * A detection offset for the sharps. @@ -223,11 +223,15 @@ move_after_moving_backward (void) /* * 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 + * 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 */ fsm_branch_t move__WAIT_FOR_CLEAR_PATH__wait_finished (void) @@ -243,9 +247,30 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) } else { - /* 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); + /* 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; + /* 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 (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_intermediate_path_found); + } } } -- cgit v1.2.3