From 23bce6a75a17b9cd9d3f35a251baa1b0635697a4 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Sat, 3 May 2008 12:43:19 +0200 Subject: * digital/io/src - manage the event where the bot can be blocked (when moving) in the different FSM, by waiting and trying again ; - ensure we update the classifier (with the servos motor) even when the arm move in a state where it should not. --- digital/io/src/move_cb.c | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 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 e85a4ef7..c55bfa30 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -157,7 +157,7 @@ move_compute_obstacle_position (asserv_position_t cur, /* Convert the angle */ uint32_t angle = cur.a; /* Change angle when going backward */ - if (asserv_get_moving_direction () == 2) + if (0) angle += 0x8000; angle = angle << 8; DPRINTF ("We are at (%d ; %d ; %x)\n", cur.x, cur.y, cur.a); @@ -236,7 +236,8 @@ move_after_moving_backward (void) fsm_branch_t move__WAIT_FOR_CLEAR_PATH__wait_finished (void) { - if (!sharp_path_obstrued (move_data.cached_moving_direction)) +/// if (!sharp_path_obstrued (move_data.cached_moving_direction)) + if (!sharp_path_obstrued (1)) { /* Try to go to the final position */ if (move_data.backward_movement_allowed) @@ -264,12 +265,12 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) else { /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_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); + return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_no_intermediate_path_found); } } } @@ -411,6 +412,8 @@ move__MOVING_BACKWARD__bot_move_failed (void) } 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 */ @@ -438,6 +441,8 @@ move__MOVING_BACKWARD__bot_move_succeed (void) } 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 */ @@ -459,7 +464,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_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); -- cgit v1.2.3