summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/ai_loader_cb.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/ai_loader_cb.c')
-rw-r--r--digital/io/src/ai_loader_cb.c55
1 files changed, 50 insertions, 5 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);
}