From 2183136d23f1b2a3eb5ee89c5902e16308036f45 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 30 Apr 2008 21:09:18 +0200 Subject: * digital/io/src - integrate backward movement support in the move FSM. --- digital/io/src/move.c | 4 +++- digital/io/src/move.h | 6 +++++- digital/io/src/move_cb.c | 28 +++++++++++++++++++++++----- digital/io/src/top_cb.c | 19 ++++++++++--------- 4 files changed, 41 insertions(+), 16 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 2c0a0c5e..51c22e87 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -34,11 +34,13 @@ struct move_data_t move_data; /* Go to a position with the start FSM. */ void -move_start (uint16_t position_x, uint16_t position_y) +move_start (uint16_t position_x, uint16_t position_y, + uint8_t backward_movement_allowed) { /* Set parameters. */ move_data.final.x = position_x; move_data.final.y = position_y; + move_data.backward_movement_allowed = backward_movement_allowed; /* Start the FSM. */ fsm_init (&move_fsm); fsm_handle_event (&move_fsm, MOVE_EVENT_start); diff --git a/digital/io/src/move.h b/digital/io/src/move.h index dc2c2603..5a328950 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -47,6 +47,8 @@ struct move_data_t move_position_t intermediate; /** Obstacle position. */ move_position_t obstacle; + /** Backward direction allowed flag. */ + uint8_t backward_movement_allowed; }; /** @@ -58,8 +60,10 @@ extern struct move_data_t move_data; * Go to a position with the start FSM. * @param position_x the X position. * @param position_y the Y position. + * @param backward_movement_allowed do we allow backward movement? */ void -move_start (uint16_t position_x, uint16_t position_y); +move_start (uint16_t position_x, uint16_t position_y, uint8_t + backward_movement_allowed); #endif /* move_h */ diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index a92135ea..7254a504 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -109,6 +109,9 @@ move_compute_obstacle_position (asserv_position_t cur, { /* Convert the angle */ uint32_t angle = cur.a; + /* Change angle when going backward */ + if (move_data.backward_movement_allowed) + angle += 0x8000; angle = angle << 8; /* X */ obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle), @@ -142,7 +145,10 @@ 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 */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + 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); } /* @@ -154,7 +160,10 @@ fsm_branch_t move__IDLE__start (void) { /* Go to the destination position */ - asserv_goto (move_data.final.x, move_data.final.y); + 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 (IDLE, start); } @@ -180,7 +189,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void) /* Get next position */ move_get_next_position (&move_data.intermediate); /* Go to the next intermediate position */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + 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); } } @@ -200,7 +212,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void) /* Get next position */ move_get_next_position (&move_data.intermediate); /* Go to the next intermediate position */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + 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 (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle); } @@ -296,7 +311,10 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void) /* Get next position */ move_get_next_position (&move_data.intermediate); /* Go to the next intermediate position */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + 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 (MOVING_TO_FINAL_POSITION, bot_move_obstacle); } diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 55b1077f..b8bf6a62 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -67,7 +67,7 @@ top__DROP_OFF_BALLS_TO_GUTTER__gutter_fsm_finished (void) asserv_get_position (&position); /* FIXME: linear move is better? */ /* Move away from the gutter */ - move_start (position.x, position.y + BOT_MIN_DISTANCE_TURN_FREE); + move_start (position.x, position.y + BOT_MIN_DISTANCE_TURN_FREE, 0); return top_next (DROP_OFF_BALLS_TO_GUTTER, gutter_fsm_finished); } @@ -102,7 +102,7 @@ fsm_branch_t top__MOVE_AWAY_FROM_START_BORDER__move_fsm_finished (void) { /* Start the move FSM to our samples distributor */ - move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y); + move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y, 0); return top_next (MOVE_AWAY_FROM_START_BORDER, move_fsm_finished); } @@ -116,7 +116,7 @@ fsm_branch_t top__GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR__get_samples_fsm_finished (void) { /* Start the move FSM to our ice distributor */ - move_start (PG_DISTRIBUTOR_ICE_OUR_X, PG_DISTRIBUTOR_ICE_OUR_Y); + move_start (PG_DISTRIBUTOR_ICE_OUR_X, PG_DISTRIBUTOR_ICE_OUR_Y, 0); return top_next (GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR, get_samples_fsm_finished); } @@ -230,13 +230,13 @@ top__GET_ICE_FROM_OUR_ICE_DISTRIBUTOR__get_samples_fsm_finished (void) if (0) { /* Start the move FSM to the adverse ice distributor */ - move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y); + move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y, 0); return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, not_full); } else { /* Start the move FSM to go to the gutter */ - move_start (PG_GUTTER_X, PG_GUTTER_Y); + move_start (PG_GUTTER_X, PG_GUTTER_Y, 1); return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, full); } } @@ -261,14 +261,15 @@ top__MOVE_AWAY_FROM_GUTTER_BORDER__move_fsm_finished (void) /* Two first loops, normal behaviour (our sample distributor, them * ice) */ /* Start the move FSM */ - move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y); + move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y, + 0); return top_next_branch (MOVE_AWAY_FROM_GUTTER_BORDER, move_fsm_finished, first_loop); } else { /* Second loops, ice only */ /* Start the move FSM */ - move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y); + move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y, 0); return top_next_branch (MOVE_AWAY_FROM_GUTTER_BORDER, move_fsm_finished, second_loop); } } @@ -283,7 +284,7 @@ fsm_branch_t top__GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR__get_samples_fsm_finished (void) { /* Start the move FSM to go to the gutter */ - move_start (PG_GUTTER_X, PG_GUTTER_Y); + move_start (PG_GUTTER_X, PG_GUTTER_Y, 1); return top_next (GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR, get_samples_fsm_finished); } @@ -312,7 +313,7 @@ top__CONFIGURE_ASSERV__settings_acknowledged (void) /* Clear the flag for the setting acknowleged */ top_waiting_for_settings_ack_ = 0; /* Start the move FSM to move the the bot away from the border */ - move_start (PG_X_START, PG_Y_START - BOT_MIN_DISTANCE_TURN_FREE); + move_start (PG_X_START, PG_Y_START - BOT_MIN_DISTANCE_TURN_FREE, 0); return top_next (CONFIGURE_ASSERV, settings_acknowledged); } -- cgit v1.2.3