summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-14 01:44:16 +0200
committerNicolas Schodet2010-05-14 01:44:16 +0200
commit6ca539dd06a20f44d6a01d450668819c7c89d201 (patch)
treef43e8fcc899304ff977a79c2ec488c660fb9fff2 /digital
parent3966014502a626b938c473535935748793c3f904 (diff)
digital/io/src: handle loader state change in any state
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_loader_cb.c55
-rw-r--r--digital/io/src/loader.c3
-rw-r--r--digital/io/src/loader.fsm12
-rw-r--r--digital/io/src/loader.h3
4 files changed, 66 insertions, 7 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index f486bbc6..5e9af297 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -223,6 +223,18 @@ ai__LOADER_UPING__elevator_failed (void)
}
/*
+ * LOADER_UPING =loader_down=>
+ * => LOADER_DOWNING
+ * move down
+ */
+fsm_branch_t
+ai__LOADER_UPING__loader_down (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_UPING, loader_down);
+}
+
+/*
* LOADER_DOWNING =elevator_succeed=>
* => LOADER_DOWN
* release elevator motor
@@ -249,6 +261,18 @@ ai__LOADER_DOWNING__elevator_failed (void)
}
/*
+ * LOADER_DOWNING =loader_up=>
+ * => LOADER_UPING
+ * move up
+ */
+fsm_branch_t
+ai__LOADER_DOWNING__loader_up (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_DOWNING, loader_up);
+}
+
+/*
* LOADER_ERROR =loader_down=>
* => LOADER_ERROR_DOWNING
* move down
@@ -430,23 +454,44 @@ ai__LOADER_LOAD_UNLOADING__elevator_failed (void)
/*
* LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=>
- * => LOADER_DOWNING
+ * down => LOADER_DOWNING
* move down
+ * up => LOADER_UPING
+ * move up
*/
fsm_branch_t
ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void)
{
- asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed);
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up);
+ }
+ else
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP,
+ BOT_ELEVATOR_SPEED);
+ return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down);
+ }
}
/*
* LOADER_LOAD_EMPTY_OPEN =clamp_succeed=>
- * => LOADER_DOWN
+ * down => LOADER_DOWN
+ * up => LOADER_UPING
+ * move up
*/
fsm_branch_t
ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void)
{
- return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_succeed);
+ if (loader_want_up)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
+ BOT_ELEVATOR_SPEED);
+ return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up);
+ }
+ else
+ return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
}
diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c
index 179cf3b8..4157023e 100644
--- a/digital/io/src/loader.c
+++ b/digital/io/src/loader.c
@@ -28,16 +28,19 @@
#include "fsm.h"
uint8_t loader_elements;
+uint8_t loader_want_up;
void
loader_up (void)
{
+ loader_want_up = 1;
fsm_handle_event (&ai_fsm, AI_EVENT_loader_up);
}
void
loader_down (void)
{
+ loader_want_up = 0;
fsm_handle_event (&ai_fsm, AI_EVENT_loader_down);
}
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 92c0d839..e830def9 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -161,12 +161,16 @@ LOADER_DOWNING:
post loader_downed event
elevator_failed -> LOADER_ERROR
post loader_errored event
+ loader_up -> LOADER_UPING
+ move up
LOADER_UPING:
elevator_succeed -> LOADER_UP
post loader_uped event
elevator_failed -> LOADER_ERROR
post loader_errored event
+ loader_down -> LOADER_DOWNING
+ move down
LOADER_LOAD_CLOSING:
clamp_succeed: full -> LOADER_LOAD_UPING
@@ -187,9 +191,13 @@ LOADER_LOAD_UNLOADING:
post loader_errored event
LOADER_LOAD_UNLOADING_OPEN:
- clamp_succeed -> LOADER_DOWNING
+ clamp_succeed: down -> LOADER_DOWNING
move down
+ clamp_succeed: up -> LOADER_UPING
+ move up
LOADER_LOAD_EMPTY_OPEN:
- clamp_succeed -> LOADER_DOWN
+ clamp_succeed: down -> LOADER_DOWN
+ clamp_succeed: up -> LOADER_UPING
+ move up
diff --git a/digital/io/src/loader.h b/digital/io/src/loader.h
index b8150891..0021a647 100644
--- a/digital/io/src/loader.h
+++ b/digital/io/src/loader.h
@@ -28,6 +28,9 @@
/** Number of loaded elements. */
extern uint8_t loader_elements;
+/** Loader up requested. */
+extern uint8_t loader_want_up;
+
/** Move loader up, in a rest position. */
void
loader_up (void);