summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/init.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/init.c')
-rw-r--r--digital/io/src/init.c206
1 files changed, 205 insertions, 1 deletions
diff --git a/digital/io/src/init.c b/digital/io/src/init.c
index 1d0d8d82..4cb2bcd9 100644
--- a/digital/io/src/init.c
+++ b/digital/io/src/init.c
@@ -22,8 +22,212 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-#include "common.h"
+#define FSM_NAME AI
+#include "common.h"
+#include "fsm.h"
+#include "asserv.h"
#include "init.h"
+#include "playground.h"
+#include "main.h"
+#include "bot.h"
+#include "switch.h"
+#include "chrono.h"
+#include "playground.h"
+#include "modules/trace/trace.h"
+#include "modules/path/path.h"
uint8_t init_match_is_started = 0;
+
+FSM_STATES (
+ /* waiting for the beginning of the top FSM. */
+ INIT_IDLE,
+ /* wait for the jack to be inserted into the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_IN,
+ /* wait the jack to be removed from the bot for the first time. */
+ INIT_WAIT_FIRST_JACK_OUT,
+ /* wait for the jack to be inserted into the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_IN,
+ /* just wait for operator hands to be removed from the jack. */
+ INIT_WAIT_FOR_HANDS_OUT,
+ /* go to the wall for the first time. */
+ INIT_GO_TO_THE_WALL,
+ /* reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL,
+ /* make the bot face the other wall. */
+ INIT_FACE_OTHER_WALL,
+ /* make sure rotation is really finished before continuing. */
+ INIT_WAIT_AFTER_ROTATION,
+ /* go to the wall for the second time. */
+ INIT_GO_TO_THE_WALL_AGAIN,
+ /* reset the X position of the bot.
+ * go backward from the wall for a specific distance. */
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ /* go to the start position. */
+ INIT_GO_TO_START_POSITION,
+ /* wait the jack to be removed from the bot for the second time. */
+ INIT_WAIT_SECOND_JACK_OUT)
+
+FSM_EVENTS (
+ /* the jack is inserted into the bot. */
+ jack_inserted_into_bot,
+ /* the jack is removed from the bot. */
+ jack_removed_from_bot)
+
+FSM_START_WITH (INIT_IDLE)
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_IDLE, start, INIT_WAIT_FIRST_JACK_IN)
+{
+ return FSM_NEXT (INIT_IDLE, start);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FIRST_JACK_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * start trace module.
+ * get and store the color of the bot.
+ */
+FSM_TRANS (INIT_WAIT_FIRST_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_WAIT_SECOND_JACK_IN)
+{
+ /* Initialize trace module (erase the flash). */
+ trace_init ();
+ /* Get the color. */
+ bot_color = switch_get_color ();
+ return FSM_NEXT (INIT_WAIT_FIRST_JACK_OUT, jack_removed_from_bot);
+}
+
+/*
+ * do nothing.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_IN,
+ jack_inserted_into_bot,
+ INIT_WAIT_FOR_HANDS_OUT)
+{
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_IN, jack_inserted_into_bot);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT, 225,
+ INIT_GO_TO_THE_WALL)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_FOR_HANDS_OUT);
+}
+
+/*
+ * reset the Y position of the bot.
+ * reset the angular position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL)
+{
+ asserv_set_y_position (PG_Y (PG_LENGTH - BOT_SIZE_FRONT));
+ asserv_set_angle_position (PG_A_DEG (90));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * turn to face the other wall.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL,
+ bot_move_succeed,
+ INIT_FACE_OTHER_WALL)
+{
+ /* Face the other wall. */
+ asserv_goto_angle (PG_A_DEG (180));
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed);
+}
+
+/*
+ * nothing to do.
+ */
+FSM_TRANS (INIT_FACE_OTHER_WALL,
+ bot_move_succeed,
+ INIT_WAIT_AFTER_ROTATION)
+{
+ return FSM_NEXT (INIT_FACE_OTHER_WALL, bot_move_succeed);
+}
+
+/*
+ * fuck the wall in front.
+ */
+FSM_TRANS_TIMEOUT (INIT_WAIT_AFTER_ROTATION, 100,
+ INIT_GO_TO_THE_WALL_AGAIN)
+{
+ /* Go to the wall, no backward. */
+ asserv_go_to_the_wall (0);
+ return FSM_NEXT_TIMEOUT (INIT_WAIT_AFTER_ROTATION);
+}
+
+/*
+ * reset the X position of the bot.
+ * move away from the wall (linear move).
+ */
+FSM_TRANS (INIT_GO_TO_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_AWAY_FROM_THE_WALL_AGAIN)
+{
+ asserv_set_x_position (PG_X (BOT_SIZE_FRONT));
+ /* Move away from the border. */
+ asserv_move_linearly (- INIT_DIST);
+ return FSM_NEXT (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * go to the start position with a go to movement.
+ */
+FSM_TRANS (INIT_GO_AWAY_FROM_THE_WALL_AGAIN,
+ bot_move_succeed,
+ INIT_GO_TO_START_POSITION)
+{
+ /* Move away from the border. */
+ asserv_goto_xya (BOT_INIT_XYA, 0);
+ return FSM_NEXT (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
+}
+
+/*
+ * nothing to do, the bot is at the start position.
+ */
+FSM_TRANS (INIT_GO_TO_START_POSITION,
+ bot_move_succeed,
+ INIT_WAIT_SECOND_JACK_OUT)
+{
+ return FSM_NEXT (INIT_GO_TO_START_POSITION, bot_move_succeed);
+}
+
+/*
+ * tell other FSM the match begins.
+ * start the chrono.
+ */
+FSM_TRANS (INIT_WAIT_SECOND_JACK_OUT,
+ jack_removed_from_bot,
+ INIT_IDLE)
+{
+ /* Set the flag to transmit to other FSM. */
+ init_match_is_started = 1;
+ /* Start the chrono. */
+ chrono_init ();
+ return FSM_NEXT (INIT_WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
+}