summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/ai_loader_cb.c6
-rw-r--r--digital/io/src/ai_top_cb.c25
-rw-r--r--digital/io/src/loader.fsm4
-rw-r--r--digital/io/src/top.fsm16
4 files changed, 34 insertions, 17 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 5bd57620..18d60c74 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -236,13 +236,13 @@ ai__LOADER_DOWNING__elevator_succeed (void)
/*
* LOADER_DOWNING =elevator_failed=>
- * => LOADER_UPING
- * something is blocking, move it up
+ * => LOADER_ERROR
+ * post loader_errored event
*/
fsm_branch_t
ai__LOADER_DOWNING__elevator_failed (void)
{
- asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ main_post_event (AI_EVENT_loader_errored);
return ai_next (LOADER_DOWNING, elevator_failed);
}
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index aa18df9d..2104c5c7 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -135,9 +135,8 @@ 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
+ * => FIRST_GO_END_OF_LINE_UNBLOCKING_UP
+ * same as below
*/
fsm_branch_t
ai__FIRST_GO_END_OF_LINE_FAST__loader_errored (void)
@@ -173,33 +172,46 @@ 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
+ * => FIRST_GO_END_OF_LINE_UNBLOCKING_UP
* move backward
- * move loader down
+ * loader up
*/
fsm_branch_t
ai__FIRST_GO_END_OF_LINE_SLOW__loader_errored (void)
{
asserv_move_linearly (-90);
- loader_down ();
+ loader_up ();
return ai_next (FIRST_GO_END_OF_LINE_SLOW, loader_errored);
}
/*
+ * FIRST_GO_END_OF_LINE_UNBLOCKING_UP =loader_uped=>
+ * => FIRST_GO_END_OF_LINE_UNBLOCKING
+ */
+fsm_branch_t
+ai__FIRST_GO_END_OF_LINE_UNBLOCKING_UP__loader_uped (void)
+{
+ return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING_UP, loader_uped);
+}
+
+/*
* FIRST_GO_END_OF_LINE_UNBLOCKING =bot_move_succeed=>
* => FIRST_GO_END_OF_LINE_SLOW
+ * move loader down
* retry
*/
fsm_branch_t
ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_succeed (void)
{
move_start_noangle (PG_VECT (2625, 253), 0);
+ loader_down ();
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
+ * move loader down
* retry
*/
fsm_branch_t
@@ -218,7 +230,6 @@ 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);
}
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 3889dd0a..cc636a97 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -159,8 +159,8 @@ LOADER_DOWNING:
elevator_succeed -> LOADER_DOWN
release elevator motor
post loader_downed event
- elevator_failed -> LOADER_UPING
- something is blocking, move it up
+ elevator_failed -> LOADER_ERROR
+ post loader_errored event
LOADER_UPING:
elevator_succeed -> LOADER_UP
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 7c7087f5..9a6b176c 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_UP
+ unblock problem with loader, move loader up
FIRST_GO_END_OF_LINE_UNBLOCKING
unblock problem with loader, go backward
UNLOAD_LOADER_UP
@@ -73,23 +75,27 @@ 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
+ loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING_UP
+ same as below
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
+ loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING_UP
move backward
- move loader down
+ loader up
+
+FIRST_GO_END_OF_LINE_UNBLOCKING_UP:
+ loader_uped -> FIRST_GO_END_OF_LINE_UNBLOCKING
FIRST_GO_END_OF_LINE_UNBLOCKING:
bot_move_succeed -> FIRST_GO_END_OF_LINE_SLOW
+ move loader down
retry
bot_move_failed -> FIRST_GO_END_OF_LINE_SLOW
+ move loader down
retry
loader_errored -> .
unblock again