From 775eaa0998d0257b6fcd55048c98fa847183892b Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 23 Apr 2008 10:56:01 +0200 Subject: * digital/io/src - add a state to the top FSM to move away from the border. --- digital/io/src/giboulee.h | 6 ++++++ digital/io/src/top.fsm | 8 +++++++- digital/io/src/top_cb.c | 21 +++++++++++++++++---- 3 files changed, 30 insertions(+), 5 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h index 2e3706c8..2d04cff2 100644 --- a/digital/io/src/giboulee.h +++ b/digital/io/src/giboulee.h @@ -75,6 +75,12 @@ # define BOT_SCALE 0.0413359788359788 #endif +/** + * Distance required to be away from a border to be able to turn freely. + * In millimeter. + */ +#define BOT_MIN_DISTANCE_TURN_FREE 300 + /** * Definition of the colors. */ diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 2a9ad8e4..cf125bb3 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -16,6 +16,8 @@ States: waiting for the jack to be removed from the bot CONFIGURE_ASSERV configuring the asserv board + MOVE_AWAY_FROM_BORDER + move the bot away from the border to be able to turn freely GO_TO_SAMPLE_DISTRIBUTOR go to our distributor of samples (using the move FSM) GO_TO_OUR_ICE_DISTRIBUTOR @@ -67,7 +69,11 @@ WAIT_JACK_OUT: set the settings of the asserv board (especially the position) CONFIGURE_ASSERV: - settings_acknowledged -> GO_TO_SAMPLE_DISTRIBUTOR + settings_acknowledged -> MOVE_AWAY_FROM_BORDER + move the bot away from the border to be able to turn freely + +MOVE_AWAY_FROM_BORDER: + move_fsm_finished -> GO_TO_SAMPLE_DISTRIBUTOR order the bot to move to our samples distributors with the move FSM GO_TO_SAMPLE_DISTRIBUTOR: diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 352fbc86..a1bdee4a 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -96,6 +96,19 @@ top__GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR__get_samples_fsm_finished (void) return top_next (GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR, get_samples_fsm_finished); } +/* + * MOVE_AWAY_FROM_BORDER =move_fsm_finished=> + * => GO_TO_SAMPLE_DISTRIBUTOR + * order the bot to move to our samples distributors with the move FSM + */ +fsm_branch_t +top__MOVE_AWAY_FROM_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); + return top_next (MOVE_AWAY_FROM_BORDER, move_fsm_finished); +} + /* * GO_TO_ADVERSE_ICE_DISTRIBUTOR =move_fsm_finished=> * => GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR @@ -222,16 +235,16 @@ top__WAIT_JACK_IN__jack_inserted_into_bot (void) /* * CONFIGURE_ASSERV =settings_acknowledged=> - * => GO_TO_SAMPLE_DISTRIBUTOR - * order the bot to move to our samples distributors with the move FSM + * => MOVE_AWAY_FROM_BORDER + * move the bot away from the border to be able to turn freely */ fsm_branch_t top__CONFIGURE_ASSERV__settings_acknowledged (void) { /* Clear the flag for the setting acknowleged */ top_waiting_for_settings_ack_ = 0; - /* Start the move FSM to our samples distributor */ - move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y); + /* 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); return top_next (CONFIGURE_ASSERV, settings_acknowledged); } -- cgit v1.2.3