summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-13 07:50:15 +0200
committerNicolas Schodet2010-05-13 07:50:15 +0200
commitbc0cafe5feed295ecae01f90ca5a4279e4896f39 (patch)
tree71be0541954e6dd0e75ba7f9eb78c36400f4cb18 /digital/io
parentc54a5b392b8aeaeb9e14983eae9be2232f6e1d44 (diff)
digital/io/src: add more blocking handling in loader FSM
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/ai_loader_cb.c104
-rw-r--r--digital/io/src/loader.fsm41
-rw-r--r--digital/io/src/main.c2
3 files changed, 142 insertions, 5 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index a51d740e..5bd57620 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -248,7 +248,7 @@ ai__LOADER_DOWNING__elevator_failed (void)
/*
* LOADER_ERROR =loader_down=>
- * => LOADER_DOWNING
+ * => LOADER_ERROR_DOWNING
* move down
*/
fsm_branch_t
@@ -260,7 +260,7 @@ ai__LOADER_ERROR__loader_down (void)
/*
* LOADER_ERROR =loader_up=>
- * => LOADER_UPING
+ * => LOADER_ERROR_UPING
* move up
*/
fsm_branch_t
@@ -271,6 +271,82 @@ ai__LOADER_ERROR__loader_up (void)
}
/*
+ * LOADER_ERROR_DOWNING =elevator_succeed=>
+ * => LOADER_ERROR_DOWNING_OPEN
+ * release elevator motor
+ * open clamp
+ */
+fsm_branch_t
+ai__LOADER_ERROR_DOWNING__elevator_succeed (void)
+{
+ asserv_motor0_free ();
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return ai_next (LOADER_ERROR_DOWNING, elevator_succeed);
+}
+
+/*
+ * LOADER_ERROR_DOWNING =elevator_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_ERROR_DOWNING__elevator_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_ERROR_DOWNING, elevator_failed);
+}
+
+/*
+ * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=>
+ * => LOADER_DOWN
+ * post loader_downed event
+ */
+fsm_branch_t
+ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void)
+{
+ main_post_event (AI_EVENT_loader_downed);
+ return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed);
+}
+
+/*
+ * LOADER_ERROR_UPING =elevator_succeed=>
+ * => LOADER_ERROR_UPING_OPEN
+ * open clamp
+ */
+fsm_branch_t
+ai__LOADER_ERROR_UPING__elevator_succeed (void)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
+ return ai_next (LOADER_ERROR_UPING, elevator_succeed);
+}
+
+/*
+ * LOADER_ERROR_UPING =elevator_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_ERROR_UPING__elevator_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_ERROR_UPING, elevator_failed);
+}
+
+/*
+ * LOADER_ERROR_UPING_OPEN =clamp_succeed=>
+ * => LOADER_UP
+ * post loader_uped event
+ */
+fsm_branch_t
+ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void)
+{
+ main_post_event (AI_EVENT_loader_uped);
+ return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed);
+}
+
+/*
* LOADER_LOAD_CLOSING =clamp_succeed=>
* full => LOADER_LOAD_UPING
* move up
@@ -312,6 +388,18 @@ ai__LOADER_LOAD_UPING__elevator_unload_position (void)
}
/*
+ * LOADER_LOAD_UPING =elevator_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_LOAD_UPING__elevator_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_LOAD_UPING, elevator_failed);
+}
+
+/*
* LOADER_LOAD_UNLOADING =elevator_succeed=>
* => LOADER_LOAD_UNLOADING_OPEN
*/
@@ -322,6 +410,18 @@ ai__LOADER_LOAD_UNLOADING__elevator_succeed (void)
}
/*
+ * LOADER_LOAD_UNLOADING =elevator_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_LOAD_UNLOADING__elevator_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_LOAD_UNLOADING, elevator_failed);
+}
+
+/*
* LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=>
* => LOADER_DOWNING
* move down
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index dd0a6a9c..3889dd0a 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -33,6 +33,14 @@ States:
moving to the down position
LOADER_ERROR
error while moving up, stop in an unknown state
+ LOADER_ERROR_DOWNING
+ error recovery moving down
+ LOADER_ERROR_DOWNING_OPEN
+ error recovery moving down, open clamp
+ LOADER_ERROR_UPING
+ error recovery moving up
+ LOADER_ERROR_UPING_OPEN
+ error recovery moving up, open clamp
LOADER_LOAD_CLOSING
close clamp
LOADER_LOAD_UPING
@@ -52,7 +60,9 @@ Events:
elevator_unload_position
elevator above unloading position
clamp_succeed
- asserv success result, clamp movement cannot fail
+ asserv success result
+ clamp_failed
+ asserv failure result
gate_succeed
asserv success result
gate_failed
@@ -119,11 +129,32 @@ LOADER_DOWN:
clamp
LOADER_ERROR:
- loader_down -> LOADER_DOWNING
+ loader_down -> LOADER_ERROR_DOWNING
move down
- loader_up -> LOADER_UPING
+ loader_up -> LOADER_ERROR_UPING
move up
+LOADER_ERROR_DOWNING:
+ elevator_succeed -> LOADER_ERROR_DOWNING_OPEN
+ release elevator motor
+ open clamp
+ elevator_failed -> LOADER_ERROR
+ post loader_errored event
+
+LOADER_ERROR_DOWNING_OPEN:
+ clamp_succeed -> LOADER_DOWN
+ post loader_downed event
+
+LOADER_ERROR_UPING:
+ elevator_succeed -> LOADER_ERROR_UPING_OPEN
+ open clamp
+ elevator_failed -> LOADER_ERROR
+ post loader_errored event
+
+LOADER_ERROR_UPING_OPEN:
+ clamp_succeed -> LOADER_UP
+ post loader_uped event
+
LOADER_DOWNING:
elevator_succeed -> LOADER_DOWN
release elevator motor
@@ -146,9 +177,13 @@ LOADER_LOAD_CLOSING:
LOADER_LOAD_UPING:
elevator_unload_position -> LOADER_LOAD_UNLOADING
open clamp
+ elevator_failed -> LOADER_ERROR
+ post loader_errored event
LOADER_LOAD_UNLOADING:
elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN
+ elevator_failed -> LOADER_ERROR
+ post loader_errored event
LOADER_LOAD_UNLOADING_OPEN:
clamp_succeed -> LOADER_DOWNING
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 6571e9a4..63de17b1 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -187,6 +187,8 @@ main_event_to_fsm (void)
if (motorm0_status == success && motorm1_status == success)
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);
+ else if (motorm0_status == failure || motorm1_status == failure)
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_failed);
/* Check positions. */
if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP)