summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorJérémy Dufour2009-05-18 13:03:58 +0200
committerJérémy Dufour2009-05-18 13:03:58 +0200
commitf5a472887d2514647ca5cd0dd8ead0790e480094 (patch)
tree697ba9b688554ccfbf095afed40a5c84bf68daa5
parent8e6335ad1ff9df290693d5f4bbf919c205d846cc (diff)
* 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.
-rw-r--r--digital/io/src/init.fsm152
-rw-r--r--digital/io/src/init_cb.c180
-rw-r--r--digital/io/src/main.c6
-rw-r--r--digital/io/src/move_cb.c2
-rw-r--r--digital/io/src/playground.h4
5 files changed, 233 insertions, 111 deletions
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.