From f5a472887d2514647ca5cd0dd8ead0790e480094 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 18 May 2009 13:03:58 +0200 Subject: * digital/io/src: - rewrite init FSM: - set X, Y, A position when doing the fuck the wall movement (thanks to Fred for the idea and Ni for debugging), - for safety reason, the procedure requires to insert the jack, remove it, insert it again. The bot will wait 2 seconds and set himself to the start position. Then you can remove the jack to make the match begin. - update name of the define for the length of the table. --- digital/io/src/init_cb.c | 180 +++++++++++++++++++++++++++++++++-------------- 1 file changed, 129 insertions(+), 51 deletions(-) (limited to 'digital/io/src/init_cb.c') diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index 4efdff82..54fff5ac 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -35,8 +35,8 @@ /* * IDLE =start=> - * => WAIT_JACK_IN - * wait for the jack to be inserted into the bot + * => WAIT_FIRST_JACK_IN + * do nothing. */ fsm_branch_t init__IDLE__start (void) @@ -45,105 +45,183 @@ init__IDLE__start (void) } /* - * WAIT_JACK_IN =jack_inserted_into_bot=> - * => WAIT_2_SEC - * wait for the operator hand disappears + * WAIT_FIRST_JACK_IN =jack_inserted_into_bot=> + * => WAIT_FIRST_JACK_OUT + * do nothing. */ fsm_branch_t -init__WAIT_JACK_IN__jack_inserted_into_bot (void) +init__WAIT_FIRST_JACK_IN__jack_inserted_into_bot (void) +{ + return init_next (WAIT_FIRST_JACK_IN, jack_inserted_into_bot); +} + +/* + * WAIT_FIRST_JACK_OUT =jack_removed_from_bot=> + * => WAIT_SECOND_JACK_IN + * start trace module. + */ +fsm_branch_t +init__WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void) { - /* Get the color. */ - bot_color = switch_get_color (); /* Initialize trace module (erase the flash). */ trace_init (); - return init_next (WAIT_JACK_IN, jack_inserted_into_bot); + return init_next (WAIT_FIRST_JACK_OUT, jack_removed_from_bot); +} + +/* + * WAIT_SECOND_JACK_IN =jack_inserted_into_bot=> + * => WAIT_FOR_HANDS_OUT + * do nothing. + */ +fsm_branch_t +init__WAIT_SECOND_JACK_IN__jack_inserted_into_bot (void) +{ + return init_next (WAIT_SECOND_JACK_IN, jack_inserted_into_bot); } /* - * WAIT_2_SEC =state_timeout=> - * => GOTO_THE_WALL - * go to the first wall + * WAIT_FOR_HANDS_OUT =state_timeout=> + * => GO_TO_THE_WALL + * fuck the wall in front. */ fsm_branch_t -init__WAIT_2_SEC__state_timeout (void) +init__WAIT_FOR_HANDS_OUT__state_timeout (void) { - /* Move forward to the wall. */ + /* Go to the wall, no backward. */ asserv_go_to_the_wall (0); - return init_next (WAIT_2_SEC, state_timeout); + return init_next (WAIT_FOR_HANDS_OUT, state_timeout); +} + +/* + * GO_TO_THE_WALL =bot_move_succeed=> + * => SET_Y_POSITION + * reset the Y position of the bot. + */ +fsm_branch_t +init__GO_TO_THE_WALL__bot_move_succeed (void) +{ + /* We are against the border of absys Y set to PG_LENGTH. */ + asserv_set_y_position (PG_LENGTH - (BOT_LENGTH / 2)); + return init_next (GO_TO_THE_WALL, bot_move_succeed); } /* - * GOTO_THE_WALL =move_done=> - * => GO_BACKWARD - * go backward for INIT_DIST millimeters + * SET_Y_POSITION =asserv_last_cmd_ack=> + * => SET_ANGULAR_POSITION + * reset the angular position of the bot. */ fsm_branch_t -init__GOTO_THE_WALL__move_done (void) +init__SET_Y_POSITION__asserv_last_cmd_ack (void) { - asserv_move_linearly (-INIT_DIST); - return init_next (GOTO_THE_WALL, move_done); + /* We are facing top border. */ + asserv_set_angle_position (90 * BOT_ANGLE_DEGREE); + return init_next (SET_Y_POSITION, asserv_last_cmd_ack); } /* - * GO_BACKWARD =move_done=> - * => TURN_90_DEGREES_CCW - * turn bot for 90 degrees counterclockwise + * SET_ANGULAR_POSITION =asserv_last_cmd_ack=> + * => GO_AWAY_FROM_THE_WALL + * move away from the wall (linear move). */ fsm_branch_t -init__GO_BACKWARD__move_done (void) +init__SET_ANGULAR_POSITION__asserv_last_cmd_ack (void) { - asserv_goto_angle (PG_A_VALUE_COMPUTING (90 * BOT_ANGLE_DEGREE)); - return init_next (GO_BACKWARD, move_done); + /* Move away from the border. */ + asserv_move_linearly (- INIT_DIST); + return init_next (SET_ANGULAR_POSITION, asserv_last_cmd_ack); } /* - * TURN_90_DEGREES_CCW =move_done=> - * => GOTO_THE_WALL_AGAIN - * go to the wall for the second time + * GO_AWAY_FROM_THE_WALL =bot_move_succeed=> + * => FACE_OTHER_WALL + * get and store the color of the bot. + * turn to face the other wall. */ fsm_branch_t -init__TURN_90_DEGREES_CCW__move_done (void) +init__GO_AWAY_FROM_THE_WALL__bot_move_succeed (void) { + /* Get the color. */ + bot_color = switch_get_color (); + /* Face the other wall. */ + asserv_goto_angle (PG_A_VALUE_COMPUTING (180 * BOT_ANGLE_DEGREE)); + return init_next (GO_AWAY_FROM_THE_WALL, bot_move_succeed); +} + +/* + * FACE_OTHER_WALL =bot_move_succeed=> + * => WAIT_AFTER_ROTATION + * nothing to do. + */ +fsm_branch_t +init__FACE_OTHER_WALL__bot_move_succeed (void) +{ + return init_next (FACE_OTHER_WALL, bot_move_succeed); +} + +/* + * WAIT_AFTER_ROTATION =state_timeout=> + * => GO_TO_THE_WALL_AGAIN + * fuck the wall in front. + */ +fsm_branch_t +init__WAIT_AFTER_ROTATION__state_timeout (void) +{ + /* Go to the wall, no backward. */ asserv_go_to_the_wall (0); - return init_next (TURN_90_DEGREES_CCW, move_done); + return init_next (WAIT_AFTER_ROTATION, state_timeout); +} + +/* + * GO_TO_THE_WALL_AGAIN =bot_move_succeed=> + * => SET_X_POSITION + * reset the X position of the bot. + */ +fsm_branch_t +init__GO_TO_THE_WALL_AGAIN__bot_move_succeed (void) +{ + asserv_set_x_position (PG_X_VALUE_COMPUTING (BOT_LENGTH / 2)); + return init_next (GO_TO_THE_WALL_AGAIN, bot_move_succeed); } /* - * GOTO_THE_WALL_AGAIN =move_done=> - * => GO_BACKWARD_AGAIN - * go backward for INIT_DIST millimeters again + * SET_X_POSITION =asserv_last_cmd_ack=> + * => GO_AWAY_FROM_THE_WALL_AGAIN + * move away from the wall (linear move). */ fsm_branch_t -init__GOTO_THE_WALL_AGAIN__move_done (void) +init__SET_X_POSITION__asserv_last_cmd_ack (void) { - asserv_move_linearly (-INIT_DIST); - return init_next (GOTO_THE_WALL_AGAIN, move_done); + /* Move away from the border. */ + asserv_move_linearly (- INIT_DIST); + return init_next (SET_X_POSITION, asserv_last_cmd_ack); } /* - * GO_BACKWARD_AGAIN =move_done=> - * => SET_POSITION - * set real position to asserv + * GO_AWAY_FROM_THE_WALL_AGAIN =bot_move_succeed=> + * => GO_TO_START_POSITION + * go to the start position with a go to movement. */ fsm_branch_t -init__GO_BACKWARD_AGAIN__move_done (void) +init__GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void) { - 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); + /* Move away from the border. */ + asserv_goto_xya (PG_X_VALUE_COMPUTING (PG_START_ZONE_WIDTH + - BOT_WIDTH / 2 + - 50), + PG_LENGTH - PG_START_ZONE_LENGTH + BOT_LENGTH / 2 + 50, + PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE)); + return init_next (GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed); } /* - * SET_POSITION =asserv_last_cmd_ack=> + * GO_TO_START_POSITION =bot_move_succeed=> * => IDLE - * turn bot for 180 degrees counterclockwise + * nothing to do, the bot is at the start position. */ fsm_branch_t -init__SET_POSITION__asserv_last_cmd_ack (void) +init__GO_TO_START_POSITION__bot_move_succeed (void) { - asserv_goto_angle (PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE)); - return init_next (SET_POSITION, asserv_last_cmd_ack); + return init_next (GO_TO_START_POSITION, bot_move_succeed); } -- cgit v1.2.3