From 6fe853aa2498e51142c4e6d799f9951a41c456d3 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Sat, 16 May 2009 21:02:59 +0200 Subject: * digital/io/src: - use timeout in init FSM rather than using main and a timer, - use the macro to compute angle and Y position by using the color contact, - use relative rotation rather than absolute, - set position before last rotation, - do some cleaning. --- digital/io/src/init.fsm | 14 +++---- digital/io/src/init.h | 6 ++- digital/io/src/init_cb.c | 101 ++++++++++++++++++++++++----------------------- 3 files changed, 61 insertions(+), 60 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm index fda5247a..8b9aa4dc 100644 --- a/digital/io/src/init.fsm +++ b/digital/io/src/init.fsm @@ -9,7 +9,7 @@ States: waiting for the beginning of the top FSM WAIT_JACK_IN waiting for the jack to be inserted into the bot - WAIT_2_SEC + WAIT_2_SEC [timeout=450] waiting for operator's hand not on jack anymore GOTO_THE_WALL go to the wall for the first time @@ -31,8 +31,6 @@ Events: initialize the FSM jack_inserted_into_bot the jack is inserted into the bot - init_tempo_ended - the tempo is finished move_done the move is finished state_timeout @@ -46,7 +44,7 @@ WAIT_JACK_IN: wait for the operator hand disappears WAIT_2_SEC: - init_tempo_ended -> GOTO_THE_WALL + state_timeout -> GOTO_THE_WALL go to the first wall GOTO_THE_WALL: @@ -66,9 +64,9 @@ GOTO_THE_WALL_AGAIN: go backward for INIT_DIST millimeters again GO_BACKWARD_AGAIN: - move_done -> TURN_180_DEGREES_CCW - turn bot for 180 degrees counterclockwise - -TURN_180_DEGREES_CCW: move_done -> SET_POSITION set real position to asserv + +SET_POSITION: + move_done -> TURN_90_DEGREES_CCW + turn bot for 180 degrees counterclockwise diff --git a/digital/io/src/init.h b/digital/io/src/init.h index 6ce25300..99de38f4 100644 --- a/digital/io/src/init.h +++ b/digital/io/src/init.h @@ -25,7 +25,9 @@ * * }}} */ -/* INIT_DIST is the distance the bot move backward during init FSM*/ -#define INIT_DIST 150 +/** + * We want to be at 50mm. + */ +#define INIT_DIST (PG_START_ZONE_LENGTH - BOT_LENGTH - 50) #endif /* init_h */ diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index 70a964c1..ff9f00c8 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -30,53 +30,54 @@ #include "playground.h" #include "main.h" #include "aquajim.h" +#include "switch.h" /* - * GOTO_THE_WALL_AGAIN =move_done=> - * => GO_BACKWARD_AGAIN - * go backward for INIT_DIST millimeters again + * IDLE =start=> + * => WAIT_JACK_IN + * wait for the jack to be inserted into the bot */ fsm_branch_t -init__GOTO_THE_WALL_AGAIN__move_done (void) +init__IDLE__start (void) { - asserv_move_linearly(-INIT_DIST); - return init_next (GOTO_THE_WALL_AGAIN, move_done); + return init_next (IDLE, start); } /* - * TURN_180_DEGREES_CCW =move_done=> - * => SET_POSITION - * set real position to asserv + * WAIT_JACK_IN =jack_inserted_into_bot=> + * => WAIT_2_SEC + * wait for the operator hand disappears */ fsm_branch_t -init__TURN_180_DEGREES_CCW__move_done (void) +init__WAIT_JACK_IN__jack_inserted_into_bot (void) { - /* FIXME Value from spa^W marcel */ - asserv_set_position(300, PG_HEIGHT - 305, 0); - return init_next (TURN_180_DEGREES_CCW, move_done); + bot_color = switch_get_color (); + return init_next (WAIT_JACK_IN, jack_inserted_into_bot); } /* - * IDLE =start=> - * => WAIT_JACK_IN - * wait for the jack to be inserted into the bot + * WAIT_2_SEC =state_timeout=> + * => GOTO_THE_WALL + * go to the first wall */ fsm_branch_t -init__IDLE__start (void) +init__WAIT_2_SEC__state_timeout (void) { - return init_next (IDLE, start); + /* Move forward to the wall. */ + asserv_go_to_the_wall (0); + return init_next (WAIT_2_SEC, state_timeout); } /* - * TURN_90_DEGREES_CCW =move_done=> - * => GOTO_THE_WALL_AGAIN - * go to the wall for the second time + * GOTO_THE_WALL =move_done=> + * => GO_BACKWARD + * go backward for INIT_DIST millimeters */ fsm_branch_t -init__TURN_90_DEGREES_CCW__move_done (void) +init__GOTO_THE_WALL__move_done (void) { - asserv_go_to_the_wall(0); - return init_next (TURN_90_DEGREES_CCW, move_done); + asserv_move_linearly (-INIT_DIST); + return init_next (GOTO_THE_WALL, move_done); } /* @@ -87,58 +88,58 @@ init__TURN_90_DEGREES_CCW__move_done (void) fsm_branch_t init__GO_BACKWARD__move_done (void) { - asserv_move_angularly(90*BOT_ANGLE_DEGREE); + asserv_goto_angle (PG_A_VALUE_COMPUTING (90 * BOT_ANGLE_DEGREE)); return init_next (GO_BACKWARD, move_done); } /* - * GOTO_THE_WALL =move_done=> - * => GO_BACKWARD - * go backward for INIT_DIST millimeters + * TURN_90_DEGREES_CCW =move_done=> + * => GOTO_THE_WALL_AGAIN + * go to the wall for the second time */ fsm_branch_t -init__GOTO_THE_WALL__move_done (void) +init__TURN_90_DEGREES_CCW__move_done (void) { - asserv_move_linearly(-INIT_DIST); - return init_next (GOTO_THE_WALL, move_done); + asserv_go_to_the_wall (0); + return init_next (TURN_90_DEGREES_CCW, move_done); } /* - * WAIT_JACK_IN =jack_inserted_into_bot=> - * => WAIT_2_SEC - * wait for the operator hand disappears + * GOTO_THE_WALL_AGAIN =move_done=> + * => GO_BACKWARD_AGAIN + * go backward for INIT_DIST millimeters again */ fsm_branch_t -init__WAIT_JACK_IN__jack_inserted_into_bot (void) +init__GOTO_THE_WALL_AGAIN__move_done (void) { - /* launch the timer (2 sec)*/ - main_init_wait_cycle = 450; - return init_next (WAIT_JACK_IN, jack_inserted_into_bot); + asserv_move_linearly (-INIT_DIST); + return init_next (GOTO_THE_WALL_AGAIN, move_done); } /* - * WAIT_2_SEC =init_tempo_ended=> - * => GOTO_THE_WALL - * go to the first wall + * GO_BACKWARD_AGAIN =move_done=> + * => SET_POSITION + * set real position to asserv */ fsm_branch_t -init__WAIT_2_SEC__init_tempo_ended (void) +init__GO_BACKWARD_AGAIN__move_done (void) { - /* tell asserv to go FORWARD to the wall */ - asserv_go_to_the_wall(0); - return init_next (WAIT_2_SEC, init_tempo_ended); + asserv_set_position (PG_X_VALUE_COMPUTING (BOT_LENGTH / 2 + INIT_DIST), + PG_HEIGHT - (BOT_WIDTH / 2 - INIT_DIST), + PG_A_VALUE_COMPUTING (180 * BOT_ANGLE_DEGREE)); + return init_next (GO_BACKWARD_AGAIN, move_done); } /* - * GO_BACKWARD_AGAIN =move_done=> - * => TURN_180_DEGREES_CCW + * SET_POSITION =move_done=> + * => TURN_90_DEGREES_CCW * turn bot for 180 degrees counterclockwise */ fsm_branch_t -init__GO_BACKWARD_AGAIN__move_done (void) +init__SET_POSITION__move_done (void) { - asserv_move_angularly(180*BOT_ANGLE_DEGREE); - return init_next (GO_BACKWARD_AGAIN, move_done); + asserv_goto_angle (PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE)); + return init_next (SET_POSITION, move_done); } -- cgit v1.2.3