summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src
diff options
context:
space:
mode:
authorJerome Jutteau2013-04-02 00:06:49 +0200
committerJerome Jutteau2013-04-02 00:14:05 +0200
commit66fb828dca770c2085946e77df859dbc4bbc5ef3 (patch)
tree898cc2e085e52f5a75494d0066197c7e6b10bf21 /digital/io-hub/src
parenta952395dac9dfd8ba28a855db8e8618944a57641 (diff)
digital/io-hub/src/apbirthday: handle init_actuators for candles
Diffstat (limited to 'digital/io-hub/src')
-rw-r--r--digital/io-hub/src/apbirthday/candles.cc30
1 files changed, 28 insertions, 2 deletions
diff --git a/digital/io-hub/src/apbirthday/candles.cc b/digital/io-hub/src/apbirthday/candles.cc
index 340c1c2e..e61f6f10 100644
--- a/digital/io-hub/src/apbirthday/candles.cc
+++ b/digital/io-hub/src/apbirthday/candles.cc
@@ -108,7 +108,9 @@ inline bool Candles::is_far (int pos)
}
/* Global candle FSM */
-FSM_STATES (AI_CANDLE_SLEEPING,
+FSM_STATES (AI_CANDLE_OFF,
+ AI_CANDLE_INIT,
+ AI_CANDLE_SLEEPING,
AI_CANDLE_READY,
AI_CANDLE_UNDEPLOYING)
@@ -116,7 +118,31 @@ FSM_EVENTS (ai_candle_deploy,
ai_candle_undeploy,
ai_candle_blow)
-FSM_START_WITH (AI_CANDLE_SLEEPING)
+FSM_START_WITH (AI_CANDLE_OFF)
+
+FSM_TRANS (AI_CANDLE_OFF, init_actuators, AI_CANDLE_INIT)
+{
+ /* Deploy for initializing. */
+ /* Deploy arm. */
+ robot->hardware.cake_arm_out.set (true);
+ robot->hardware.cake_arm_in.set (false);
+ /* Prepare far pusher. */
+ robot->hardware.cake_push_far_out.set (false);
+ robot->hardware.cake_push_far_in.set (true);
+ /* Prepare near pusher. */
+ robot->hardware.cake_push_near_out.set (false);
+ robot->hardware.cake_push_near_in.set (true);
+}
+
+FSM_TRANS_TIMEOUT (AI_CANDLE_INIT, 12, AI_CANDLE_UNDEPLOYING)
+{
+ /* Prepare far puncher to undeploy. */
+ robot->hardware.cake_push_far_out.set (true);
+ robot->hardware.cake_push_far_in.set (false);
+ /* Be sure the near punched in not punching. */
+ robot->hardware.cake_push_near_out.set (false);
+ robot->hardware.cake_push_near_in.set (true);
+}
FSM_TRANS (AI_CANDLE_SLEEPING, ai_candle_deploy, AI_CANDLE_READY)
{