summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-12 20:38:15 +0200
committerNicolas Schodet2010-05-12 20:38:15 +0200
commitc17d0d5431258c7c5a006ddeffbb0dad1592528f (patch)
tree7983203abeb23be6c6fd4fa6787c85f2c4991c21
parent994bafe320fc7d0847ce8e4652a85aaee9a543eb (diff)
digital/io/src: add gate init
-rw-r--r--digital/io/src/ai_loader_cb.c50
-rw-r--r--digital/io/src/bot.h6
-rw-r--r--digital/io/src/loader.fsm31
-rw-r--r--digital/io/src/main.c5
4 files changed, 88 insertions, 4 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
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index df99aacb..f0264240 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -110,6 +110,12 @@
/** Clamp open loop PWM. */
#define BOT_CLAMP_PWM 0x100
+/** Gate work speed. */
+#define BOT_GATE_SPEED 0x46
+
+/** Gate stroke in steps. */
+#define BOT_GATE_STROKE_STEP 0x2081
+
/**
* Definition of the colors.
*/
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 98b18da2..1978ef41 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -15,8 +15,16 @@ States:
test clamp closing
LOADER_INIT_CLAMP_ZERO
find clamp zero position
+ LOADER_INIT_GATE_ZERO
+ find gate zero
LOADER_INIT_ELEVATOR_UP
move the elevator up so that the robot can go to the wall
+ LOADER_INIT_GATE_OPEN
+ open gate to unload elements before start
+ LOADER_INIT_GATE_WAIT[timeout=225]
+ wait with gate open
+ LOADER_INIT_GATE_CLOSE
+ close gate before start
LOADER_UP
up position, clamp is protected
LOADER_DOWN
@@ -47,6 +55,10 @@ Events:
elevator above unloading position
clamp_succeed
asserv success result, clamp movement cannot fail
+ gate_succeed
+ asserv success result
+ gate_failed
+ asserv failure result
loader_element
element sensed between clamp to be loaded
loader_down
@@ -70,6 +82,7 @@ LOADER_WAIT_JACK_OUT:
jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO
find elevator zero
close clamp
+ find gate zero
LOADER_INIT_ELEVATOR_ZERO:
elevator_succeed -> LOADER_INIT_CLAMP_CLOSE
@@ -80,13 +93,27 @@ LOADER_INIT_CLAMP_CLOSE:
find clamp zero
LOADER_INIT_CLAMP_ZERO:
- clamp_succeed -> LOADER_INIT_ELEVATOR_UP
+ clamp_succeed -> LOADER_INIT_GATE_ZERO
+
+LOADER_INIT_GATE_ZERO:
+ gate_succeed -> LOADER_INIT_ELEVATOR_UP
LOADER_INIT_ELEVATOR_UP:
- elevator_succeed -> LOADER_UP
+ elevator_succeed -> LOADER_INIT_GATE_OPEN
+ open gate
elevator_failed -> LOADER_IDLE
initialisation failure
+LOADER_INIT_GATE_OPEN:
+ gate_succeed -> LOADER_INIT_GATE_WAIT
+
+LOADER_INIT_GATE_WAIT:
+ state_timeout -> LOADER_INIT_GATE_CLOSE
+ close gate
+
+LOADER_INIT_GATE_CLOSE:
+ gate_succeed -> LOADER_UP
+
LOADER_UP:
loader_down -> LOADER_DOWNING
move down
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index f33afbb5..f05653eb 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -150,6 +150,11 @@ main_event_to_fsm (void)
else if (motor0_status == failure)
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed);
+ if (motor1_status == success)
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_succeed);
+ else if (motor1_status == failure)
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_failed);
+
if (motorm0_status == success && motorm1_status == success)
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);