summaryrefslogtreecommitdiff
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.c50
1 files changed, 48 insertions, 2 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 7f1a6073..3a2782ab 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -55,6 +55,7 @@ ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void)
* => LOADER_INIT_ELEVATOR_ZERO
* find elevator zero
* close clamp
+ * find gate zero
*/
fsm_branch_t
ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void)
@@ -62,6 +63,7 @@ ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void)
asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0);
mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0);
+ asserv_motor1_zero_position (BOT_GATE_SPEED);
return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
}
@@ -93,7 +95,7 @@ ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void)
/*
* LOADER_INIT_CLAMP_ZERO =clamp_succeed=>
- * => LOADER_INIT_ELEVATOR_UP
+ * => LOADER_INIT_GATE_ZERO
*/
fsm_branch_t
ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void)
@@ -102,12 +104,24 @@ ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void)
}
/*
+ * LOADER_INIT_GATE_ZERO =gate_succeed=>
+ * => LOADER_INIT_ELEVATOR_UP
+ */
+fsm_branch_t
+ai__LOADER_INIT_GATE_ZERO__gate_succeed (void)
+{
+ return ai_next (LOADER_INIT_GATE_ZERO, gate_succeed);
+}
+
+/*
* LOADER_INIT_ELEVATOR_UP =elevator_succeed=>
- * => LOADER_UP
+ * => LOADER_INIT_GATE_OPEN
+ * open gate
*/
fsm_branch_t
ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void)
{
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed);
}
@@ -126,6 +140,38 @@ ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void)
}
/*
+ * LOADER_INIT_GATE_OPEN =gate_succeed=>
+ * => LOADER_INIT_GATE_WAIT
+ */
+fsm_branch_t
+ai__LOADER_INIT_GATE_OPEN__gate_succeed (void)
+{
+ return ai_next (LOADER_INIT_GATE_OPEN, gate_succeed);
+}
+
+/*
+ * LOADER_INIT_GATE_WAIT =state_timeout=>
+ * => LOADER_INIT_GATE_CLOSE
+ * close gate
+ */
+fsm_branch_t
+ai__LOADER_INIT_GATE_WAIT__state_timeout (void)
+{
+ asserv_move_motor1_absolute (0, BOT_GATE_SPEED);
+ return ai_next (LOADER_INIT_GATE_WAIT, state_timeout);
+}
+
+/*
+ * LOADER_INIT_GATE_CLOSE =gate_succeed=>
+ * => LOADER_UP
+ */
+fsm_branch_t
+ai__LOADER_INIT_GATE_CLOSE__gate_succeed (void)
+{
+ return ai_next (LOADER_INIT_GATE_CLOSE, gate_succeed);
+}
+
+/*
* LOADER_UP =loader_down=>
* => LOADER_DOWNING
* move down