summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/init.fsm14
-rw-r--r--digital/io/src/init.h6
-rw-r--r--digital/io/src/init_cb.c101
3 files changed, 61 insertions, 60 deletions
diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm
index fda5247a..8b9aa4dc 100644
--- a/digital/io/src/init.fsm
+++ b/digital/io/src/init.fsm
@@ -9,7 +9,7 @@ States:
waiting for the beginning of the top FSM
WAIT_JACK_IN
waiting for the jack to be inserted into the bot
- WAIT_2_SEC
+ 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
@@ -31,8 +31,6 @@ Events:
initialize the FSM
jack_inserted_into_bot
the jack is inserted into the bot
- init_tempo_ended
- the tempo is finished
move_done
the move is finished
state_timeout
@@ -46,7 +44,7 @@ WAIT_JACK_IN:
wait for the operator hand disappears
WAIT_2_SEC:
- init_tempo_ended -> GOTO_THE_WALL
+ state_timeout -> GOTO_THE_WALL
go to the first wall
GOTO_THE_WALL:
@@ -66,9 +64,9 @@ GOTO_THE_WALL_AGAIN:
go backward for INIT_DIST millimeters again
GO_BACKWARD_AGAIN:
- move_done -> TURN_180_DEGREES_CCW
- turn bot for 180 degrees counterclockwise
-
-TURN_180_DEGREES_CCW:
move_done -> SET_POSITION
set real position to asserv
+
+SET_POSITION:
+ move_done -> TURN_90_DEGREES_CCW
+ turn bot for 180 degrees counterclockwise
diff --git a/digital/io/src/init.h b/digital/io/src/init.h
index 6ce25300..99de38f4 100644
--- a/digital/io/src/init.h
+++ b/digital/io/src/init.h
@@ -25,7 +25,9 @@
*
* }}} */
-/* INIT_DIST is the distance the bot move backward during init FSM*/
-#define INIT_DIST 150
+/**
+ * We want to be at 50mm.
+ */
+#define INIT_DIST (PG_START_ZONE_LENGTH - BOT_LENGTH - 50)
#endif /* init_h */
diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c
index 70a964c1..ff9f00c8 100644
--- a/digital/io/src/init_cb.c
+++ b/digital/io/src/init_cb.c
@@ -30,53 +30,54 @@
#include "playground.h"
#include "main.h"
#include "aquajim.h"
+#include "switch.h"
/*
- * GOTO_THE_WALL_AGAIN =move_done=>
- * => GO_BACKWARD_AGAIN
- * go backward for INIT_DIST millimeters again
+ * IDLE =start=>
+ * => WAIT_JACK_IN
+ * wait for the jack to be inserted into the bot
*/
fsm_branch_t
-init__GOTO_THE_WALL_AGAIN__move_done (void)
+init__IDLE__start (void)
{
- asserv_move_linearly(-INIT_DIST);
- return init_next (GOTO_THE_WALL_AGAIN, move_done);
+ return init_next (IDLE, start);
}
/*
- * TURN_180_DEGREES_CCW =move_done=>
- * => SET_POSITION
- * set real position to asserv
+ * WAIT_JACK_IN =jack_inserted_into_bot=>
+ * => WAIT_2_SEC
+ * wait for the operator hand disappears
*/
fsm_branch_t
-init__TURN_180_DEGREES_CCW__move_done (void)
+init__WAIT_JACK_IN__jack_inserted_into_bot (void)
{
- /* FIXME Value from spa^W marcel */
- asserv_set_position(300, PG_HEIGHT - 305, 0);
- return init_next (TURN_180_DEGREES_CCW, move_done);
+ bot_color = switch_get_color ();
+ return init_next (WAIT_JACK_IN, jack_inserted_into_bot);
}
/*
- * IDLE =start=>
- * => WAIT_JACK_IN
- * wait for the jack to be inserted into the bot
+ * WAIT_2_SEC =state_timeout=>
+ * => GOTO_THE_WALL
+ * go to the first wall
*/
fsm_branch_t
-init__IDLE__start (void)
+init__WAIT_2_SEC__state_timeout (void)
{
- return init_next (IDLE, start);
+ /* Move forward to the wall. */
+ asserv_go_to_the_wall (0);
+ return init_next (WAIT_2_SEC, state_timeout);
}
/*
- * TURN_90_DEGREES_CCW =move_done=>
- * => GOTO_THE_WALL_AGAIN
- * go to the wall for the second time
+ * GOTO_THE_WALL =move_done=>
+ * => GO_BACKWARD
+ * go backward for INIT_DIST millimeters
*/
fsm_branch_t
-init__TURN_90_DEGREES_CCW__move_done (void)
+init__GOTO_THE_WALL__move_done (void)
{
- asserv_go_to_the_wall(0);
- return init_next (TURN_90_DEGREES_CCW, move_done);
+ asserv_move_linearly (-INIT_DIST);
+ return init_next (GOTO_THE_WALL, move_done);
}
/*
@@ -87,58 +88,58 @@ init__TURN_90_DEGREES_CCW__move_done (void)
fsm_branch_t
init__GO_BACKWARD__move_done (void)
{
- asserv_move_angularly(90*BOT_ANGLE_DEGREE);
+ asserv_goto_angle (PG_A_VALUE_COMPUTING (90 * BOT_ANGLE_DEGREE));
return init_next (GO_BACKWARD, move_done);
}
/*
- * GOTO_THE_WALL =move_done=>
- * => GO_BACKWARD
- * go backward for INIT_DIST millimeters
+ * TURN_90_DEGREES_CCW =move_done=>
+ * => GOTO_THE_WALL_AGAIN
+ * go to the wall for the second time
*/
fsm_branch_t
-init__GOTO_THE_WALL__move_done (void)
+init__TURN_90_DEGREES_CCW__move_done (void)
{
- asserv_move_linearly(-INIT_DIST);
- return init_next (GOTO_THE_WALL, move_done);
+ asserv_go_to_the_wall (0);
+ return init_next (TURN_90_DEGREES_CCW, move_done);
}
/*
- * WAIT_JACK_IN =jack_inserted_into_bot=>
- * => WAIT_2_SEC
- * wait for the operator hand disappears
+ * GOTO_THE_WALL_AGAIN =move_done=>
+ * => GO_BACKWARD_AGAIN
+ * go backward for INIT_DIST millimeters again
*/
fsm_branch_t
-init__WAIT_JACK_IN__jack_inserted_into_bot (void)
+init__GOTO_THE_WALL_AGAIN__move_done (void)
{
- /* launch the timer (2 sec)*/
- main_init_wait_cycle = 450;
- return init_next (WAIT_JACK_IN, jack_inserted_into_bot);
+ asserv_move_linearly (-INIT_DIST);
+ return init_next (GOTO_THE_WALL_AGAIN, move_done);
}
/*
- * WAIT_2_SEC =init_tempo_ended=>
- * => GOTO_THE_WALL
- * go to the first wall
+ * GO_BACKWARD_AGAIN =move_done=>
+ * => SET_POSITION
+ * set real position to asserv
*/
fsm_branch_t
-init__WAIT_2_SEC__init_tempo_ended (void)
+init__GO_BACKWARD_AGAIN__move_done (void)
{
- /* tell asserv to go FORWARD to the wall */
- asserv_go_to_the_wall(0);
- return init_next (WAIT_2_SEC, init_tempo_ended);
+ 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);
}
/*
- * GO_BACKWARD_AGAIN =move_done=>
- * => TURN_180_DEGREES_CCW
+ * SET_POSITION =move_done=>
+ * => TURN_90_DEGREES_CCW
* turn bot for 180 degrees counterclockwise
*/
fsm_branch_t
-init__GO_BACKWARD_AGAIN__move_done (void)
+init__SET_POSITION__move_done (void)
{
- asserv_move_angularly(180*BOT_ANGLE_DEGREE);
- return init_next (GO_BACKWARD_AGAIN, move_done);
+ asserv_goto_angle (PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE));
+ return init_next (SET_POSITION, move_done);
}