summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/top.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/top.c')
-rw-r--r--digital/io/src/top.c359
1 files changed, 359 insertions, 0 deletions
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index 5bfeaa4d..bc6eb20e 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -22,6 +22,10 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+
+#define FSM_NAME AI
+
+#include "fsm.h"
#include "common.h"
#include "defs.h"
#include "top.h"
@@ -88,3 +92,358 @@ top_collect (uint8_t force)
}
}
+FSM_INIT
+
+FSM_STATES (IDLE,
+ WAIT_INIT_TO_FINISH,
+ FIRST_GO_BEGIN_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_FAST,
+ FIRST_GO_END_OF_LINE_SLOW,
+ UNLOAD,
+ UNLOAD_LOADER_UP,
+ UNLOAD_FACE_BIN,
+ UNLOAD_FACE_BIN_UNBLOCK,
+ UNLOAD_BACK_BIN,
+ UNLOAD_UNLOAD,
+ COLLECT,
+ COLLECT_SLOW_MOTION,
+ COLLECT_BLACK)
+
+FSM_EVENTS (start,
+ move_fsm_succeed,
+ move_fsm_failed,
+ bot_move_succeed,
+ bot_move_failed,
+ init_match_is_started,
+ in_field,
+ asserv_last_cmd_ack,
+ clamp_succeed)
+
+FSM_START_WITH (IDLE)
+
+/* Nothing to do. */
+FSM_TRANS (IDLE, start, WAIT_INIT_TO_FINISH)
+{
+ return FSM_NEXT (IDLE, start);
+}
+
+/*
+ * the match start
+ * set fast speed
+ * go to first sequence begin of line
+ */
+FSM_TRANS (WAIT_INIT_TO_FINISH,
+ init_match_is_started,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_FAST);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (WAIT_INIT_TO_FINISH, init_match_is_started);
+}
+
+/*
+ * go to end of line
+ * loader down
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_succeed,
+ FIRST_GO_END_OF_LINE_FAST)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ loader_down ();
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_BEGIN_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_BEGIN_OF_LINE_FAST)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (375, 1503), 0, 0);
+ return FSM_NEXT (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * set slow speed
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ in_field,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, in_field);
+}
+
+/*
+ * set slow speed
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed);
+}
+
+/*
+ * set slow speed
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_FAST,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_succeed,
+ UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (FIRST_GO_END_OF_LINE_SLOW,
+ move_fsm_failed,
+ FIRST_GO_END_OF_LINE_SLOW)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
+}
+
+/*
+ * move loader up
+ */
+FSM_TRANS (UNLOAD, move_fsm_succeed, UNLOAD_LOADER_UP)
+{
+ loader_up ();
+ return FSM_NEXT (UNLOAD, move_fsm_succeed);
+}
+
+/*
+ * retry
+ */
+FSM_TRANS (UNLOAD, move_fsm_failed, UNLOAD)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return FSM_NEXT (UNLOAD, move_fsm_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_uped, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_uped);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_LOADER_UP, loader_errored, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_LOADER_UP, loader_errored);
+}
+
+/*
+ * go backward to bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_succeed, UNLOAD_BACK_BIN)
+{
+ asserv_move_linearly (-(128 + 250 / 2 - BOT_SIZE_BACK - 50));
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_succeed);
+}
+
+/*
+ * move backward
+ */
+FSM_TRANS (UNLOAD_FACE_BIN, bot_move_failed, UNLOAD_FACE_BIN_UNBLOCK)
+{
+ asserv_move_linearly (-40);
+ return FSM_NEXT (UNLOAD_FACE_BIN, bot_move_failed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
+}
+
+/*
+ * turn toward bin
+ */
+FSM_TRANS (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed, UNLOAD_FACE_BIN)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return FSM_NEXT (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_succeed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_succeed);
+}
+
+/*
+ * unload
+ */
+FSM_TRANS (UNLOAD_BACK_BIN, bot_move_failed, UNLOAD_UNLOAD)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return FSM_NEXT (UNLOAD_BACK_BIN, bot_move_failed);
+}
+
+/*
+ * close gate
+ * loader down
+ * choose best food to collect
+ */
+FSM_TRANS_TIMEOUT (UNLOAD_UNLOAD, 255, COLLECT)
+{
+ loader_elements = 0;
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ loader_down ();
+ top_collect (1);
+ return FSM_NEXT_TIMEOUT (UNLOAD_UNLOAD);
+}
+
+/*
+ * post loader_element event
+ */
+FSM_TRANS (COLLECT, move_fsm_succeed,
+ unload, UNLOAD,
+ collect, COLLECT,
+ slow_motion, COLLECT_SLOW_MOTION)
+{
+ int16_t slow_motion = food_slow_motion (top_food);
+ if (slow_motion)
+ {
+ FSM_HANDLE (AI, loader_element);
+ return FSM_NEXT (COLLECT, move_fsm_succeed, slow_motion);
+ }
+ else if (top_collect (0))
+ return FSM_NEXT (COLLECT, move_fsm_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT, move_fsm_succeed, unload);
+}
+
+/*
+ * cancel this element
+ */
+FSM_TRANS (COLLECT, move_fsm_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ {
+ food_cancel (top_food);
+ return FSM_NEXT (COLLECT, move_fsm_failed, collect);
+ }
+ else
+ return FSM_NEXT (COLLECT, move_fsm_failed, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT, loader_black);
+}
+
+/*
+ * collect
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_downed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_downed, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_errored,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, collect);
+ else
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_errored, unload);
+}
+
+/*
+ * same as above
+ */
+FSM_TRANS_TIMEOUT (COLLECT_SLOW_MOTION, 450,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, collect);
+ else
+ return FSM_NEXT_TIMEOUT (COLLECT_SLOW_MOTION, unload);
+}
+
+/*
+ * move backward
+ * mark as black
+ */
+FSM_TRANS (COLLECT_SLOW_MOTION, loader_black, COLLECT_BLACK)
+{
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return FSM_NEXT (COLLECT_SLOW_MOTION, loader_black);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_succeed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_succeed, unload);
+}
+
+FSM_TRANS (COLLECT_BLACK, bot_move_failed,
+ unload, UNLOAD,
+ collect, COLLECT)
+{
+ if (top_collect (0))
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, collect);
+ else
+ return FSM_NEXT (COLLECT_BLACK, bot_move_failed, unload);
+}
+