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.fsm | 152 +++++++++++++++++++++++-------------- digital/io/src/init_cb.c | 180 +++++++++++++++++++++++++++++++------------- digital/io/src/main.c | 6 +- digital/io/src/move_cb.c | 2 +- digital/io/src/playground.h | 4 +- 5 files changed, 233 insertions(+), 111 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm index cbd2233d..c1700e50 100644 --- a/digital/io/src/init.fsm +++ b/digital/io/src/init.fsm @@ -1,77 +1,119 @@ # init FSM -# This FSM set the position of the bot with some move on wall -# This FSM is required because the meca team is "relou" +# This FSM is used to place the bot at a "start position" for a match. It is +# more precise than a human hand and reset the asserv board to the start +# position. +# Some remarks: +# - for security reason, we need to insert the jack then removed it, then +# insert it back. Why? Because it can already be inserted. +# - other FSM can do a similar thing by using the first jack out to +# initialize. init - init FSM with setup move + place the bot in the start position for a match. States: IDLE - waiting for the beginning of the top FSM - WAIT_JACK_IN - waiting for the jack to be inserted into the bot. - get color for the rest of the FSM. - erase flash. - 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 - GO_BACKWARD - go backward for INIT_DIST millimeters - TURN_90_DEGREES_CCW - turn bot for 90 degrees counterclockwise - GOTO_THE_WALL_AGAIN - go to the wall for the second time - GO_BACKWARD_AGAIN - go backward for INIT_DIST millimeters again - TURN_180_DEGREES_CCW - turn bot for 180 degrees counterclockwise - SET_POSITION - tell the truth to asserv + waiting for the beginning of the top FSM. + WAIT_FIRST_JACK_IN + wait for the jack to be inserted into the bot for the first time. + WAIT_FIRST_JACK_OUT + wait the jack to be removed from the bot for the first time. + WAIT_SECOND_JACK_IN + wait for the jack to be inserted into the bot for the second time. + WAIT_FOR_HANDS_OUT[timeout=450] + just wait for operator hands to be removed from the jack. + GO_TO_THE_WALL + go to the wall for the first time. + SET_Y_POSITION + reset the Y position of the bot. + SET_ANGULAR_POSITION + reset the angular position of the bot. + GO_AWAY_FROM_THE_WALL + go backward from the wall for a specific distance. + FACE_OTHER_WALL + make the bot face the other wall. + WAIT_AFTER_ROTATION [timeout=100] + make sure rotation is really finished before continuing. + GO_TO_THE_WALL_AGAIN + go to the wall for the second time. + SET_X_POSITION + reset the X position of the bot. + GO_AWAY_FROM_THE_WALL_AGAIN + go backward from the wall for a specific distance. + GO_TO_START_POSITION + go to the start position. Events: start - initialize the FSM + initialize the FSM. jack_inserted_into_bot - the jack is inserted into the bot - move_done - the move is finished + the jack is inserted into the bot. + jack_removed_from_bot + the jack is removed from the bot. + bot_move_succeed + the move is finished. state_timeout timeout. asserv_last_cmd_ack - last command sent to the asserv board acquited. + last command sent to the asserv board has been acquitted. IDLE: - start -> WAIT_JACK_IN - wait for the jack to be inserted into the bot + start -> WAIT_FIRST_JACK_IN + do nothing. -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. -WAIT_2_SEC: - state_timeout -> GOTO_THE_WALL - go to the first wall +WAIT_FIRST_JACK_OUT: + jack_removed_from_bot -> WAIT_SECOND_JACK_IN + start trace module. -GOTO_THE_WALL: - move_done -> GO_BACKWARD - go backward for INIT_DIST millimeters +WAIT_SECOND_JACK_IN: + jack_inserted_into_bot -> WAIT_FOR_HANDS_OUT + do nothing. -GO_BACKWARD: - move_done -> TURN_90_DEGREES_CCW - turn bot for 90 degrees counterclockwise +WAIT_FOR_HANDS_OUT: + state_timeout -> GO_TO_THE_WALL + fuck the wall in front. -TURN_90_DEGREES_CCW: - move_done -> GOTO_THE_WALL_AGAIN - go to the wall for the second time +GO_TO_THE_WALL: + bot_move_succeed -> SET_Y_POSITION + reset the Y position of the bot. -GOTO_THE_WALL_AGAIN: - move_done -> GO_BACKWARD_AGAIN - go backward for INIT_DIST millimeters again +SET_Y_POSITION: + asserv_last_cmd_ack -> SET_ANGULAR_POSITION + reset the angular position of the bot. -GO_BACKWARD_AGAIN: - move_done -> SET_POSITION - set real position to asserv +SET_ANGULAR_POSITION: + asserv_last_cmd_ack -> GO_AWAY_FROM_THE_WALL + move away from the wall (linear move). + +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. + +FACE_OTHER_WALL: + bot_move_succeed -> WAIT_AFTER_ROTATION + nothing to do. + +WAIT_AFTER_ROTATION: + state_timeout -> GO_TO_THE_WALL_AGAIN + fuck the wall in front. + +GO_TO_THE_WALL_AGAIN: + bot_move_succeed -> SET_X_POSITION + reset the X position of the bot. + +SET_X_POSITION: + asserv_last_cmd_ack -> GO_AWAY_FROM_THE_WALL_AGAIN + move away from the wall (linear move). + +GO_AWAY_FROM_THE_WALL_AGAIN: + bot_move_succeed -> GO_TO_START_POSITION + go to the start position with a go to movement. + +GO_TO_START_POSITION: + bot_move_succeed -> IDLE + nothing to do, the bot is at the start position. -SET_POSITION: - asserv_last_cmd_ack -> IDLE - turn bot for 180 degrees counterclockwise 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); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 6bd2d14b..1384be2f 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -164,7 +164,7 @@ main_event_to_fsm (void) FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_succeed); FSM_HANDLE_EVENT (&init_fsm, - INIT_EVENT_move_done); + INIT_EVENT_bot_move_succeed); } else if (move_status == failure) { @@ -216,6 +216,8 @@ main_event_to_fsm (void) { FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_jack_removed_from_bot); + FSM_HANDLE_EVENT (&init_fsm, + INIT_EVENT_jack_removed_from_bot); } else { @@ -309,7 +311,7 @@ main_init (void) switch_init (); /* Path module */ path_init (PG_BORDER_DISTANCE, PG_BORDER_DISTANCE, - PG_WIDTH - PG_BORDER_DISTANCE, PG_HEIGHT - PG_BORDER_DISTANCE); + PG_WIDTH - PG_BORDER_DISTANCE, PG_LENGTH - PG_BORDER_DISTANCE); /* Init all FSM (except move FSM) */ fsm_init(&top_fsm); fsm_init(&init_fsm); diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index ec5f6715..abf22c74 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -96,7 +96,7 @@ move_obstacle_in_table (move_position_t pos) { if ((pos.x <= PG_WIDTH - MOVE_DETECTION_OFFSET) && (pos.x > MOVE_DETECTION_OFFSET) - && (pos.y <= PG_HEIGHT- MOVE_DETECTION_OFFSET) + && (pos.y <= PG_LENGTH - MOVE_DETECTION_OFFSET) && (pos.y > MOVE_DETECTION_OFFSET)) return 0x1; else diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index 41788a4e..21060d3b 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -42,9 +42,9 @@ #define PG_WIDTH 3000 /** - * The height of the table, in millimeters. + * The length of the table, in millimeters. */ -#define PG_HEIGHT 2100 +#define PG_LENGTH 2100 /** * The distance from table border for movements. -- cgit v1.2.3