summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-13 08:09:14 +0200
committerNicolas Schodet2010-05-13 08:09:14 +0200
commit81c3639247293714de5913f283b786bc73cba491 (patch)
tree491a6e8be2115df1112a8544281c16b12d966bc7
parentbc0cafe5feed295ecae01f90ca5a4279e4896f39 (diff)
digital/io/src: handle loader blocking in top FSM
-rw-r--r--digital/io/src/ai_top_cb.c65
-rw-r--r--digital/io/src/top.fsm16
2 files changed, 81 insertions, 0 deletions
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index 6e0a0c8d..aa18df9d 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -134,6 +134,20 @@ ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void)
}
/*
+ * FIRST_GO_END_OF_LINE_FAST =loader_errored=>
+ * => FIRST_GO_END_OF_LINE_UNBLOCKING
+ * move backward
+ * move loader down
+ */
+fsm_branch_t
+ai__FIRST_GO_END_OF_LINE_FAST__loader_errored (void)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ ai__FIRST_GO_END_OF_LINE_SLOW__loader_errored ();
+ return ai_next (FIRST_GO_END_OF_LINE_FAST, loader_errored);
+}
+
+/*
* FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=>
* => UNLOAD_LOADER_UP
* move loader up
@@ -158,6 +172,57 @@ ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void)
}
/*
+ * FIRST_GO_END_OF_LINE_SLOW =loader_errored=>
+ * => FIRST_GO_END_OF_LINE_UNBLOCKING
+ * move backward
+ * move loader down
+ */
+fsm_branch_t
+ai__FIRST_GO_END_OF_LINE_SLOW__loader_errored (void)
+{
+ asserv_move_linearly (-90);
+ loader_down ();
+ return ai_next (FIRST_GO_END_OF_LINE_SLOW, loader_errored);
+}
+
+/*
+ * FIRST_GO_END_OF_LINE_UNBLOCKING =bot_move_succeed=>
+ * => FIRST_GO_END_OF_LINE_SLOW
+ * retry
+ */
+fsm_branch_t
+ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_succeed (void)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0);
+ return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, bot_move_succeed);
+}
+
+/*
+ * FIRST_GO_END_OF_LINE_UNBLOCKING =bot_move_failed=>
+ * => FIRST_GO_END_OF_LINE_SLOW
+ * retry
+ */
+fsm_branch_t
+ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_failed (void)
+{
+ ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_succeed ();
+ return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, bot_move_failed);
+}
+
+/*
+ * FIRST_GO_END_OF_LINE_UNBLOCKING =loader_errored=>
+ * => FIRST_GO_END_OF_LINE_UNBLOCKING
+ * unblock again
+ */
+fsm_branch_t
+ai__FIRST_GO_END_OF_LINE_UNBLOCKING__loader_errored (void)
+{
+ asserv_move_linearly (-90);
+ loader_down ();
+ return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, loader_errored);
+}
+
+/*
* UNLOAD_LOADER_UP =loader_uped=>
* => UNLOAD_FACE_BIN
* turn toward bin
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 9a61d00c..7c7087f5 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -13,6 +13,8 @@ States:
first sequence, go to end of line
FIRST_GO_END_OF_LINE_SLOW
slow down to take elements
+ FIRST_GO_END_OF_LINE_UNBLOCKING
+ unblock problem with loader, go backward
UNLOAD_LOADER_UP
move loader to up position
UNLOAD_FACE_BIN
@@ -71,12 +73,26 @@ FIRST_GO_END_OF_LINE_FAST:
move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
set slow speed
retry
+ loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING
+ move backward
+ move loader down
FIRST_GO_END_OF_LINE_SLOW:
move_fsm_succeed -> UNLOAD_LOADER_UP
move loader up
move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW
retry
+ loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING
+ move backward
+ move loader down
+
+FIRST_GO_END_OF_LINE_UNBLOCKING:
+ bot_move_succeed -> FIRST_GO_END_OF_LINE_SLOW
+ retry
+ bot_move_failed -> FIRST_GO_END_OF_LINE_SLOW
+ retry
+ loader_errored -> .
+ unblock again
UNLOAD_LOADER_UP:
loader_uped -> UNLOAD_FACE_BIN