From 66fb828dca770c2085946e77df859dbc4bbc5ef3 Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Tue, 2 Apr 2013 00:06:49 +0200 Subject: digital/io-hub/src/apbirthday: handle init_actuators for candles --- digital/io-hub/src/apbirthday/candles.cc | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) (limited to 'digital/io-hub/src/apbirthday') 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) { -- cgit v1.2.3