summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/init_cb.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/init_cb.c')
-rw-r--r--digital/io/src/init_cb.c180
1 files changed, 129 insertions, 51 deletions
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);
}