summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/apbirthday/candles.cc
diff options
context:
space:
mode:
authorJerome Jutteau2013-04-21 06:35:16 +0200
committerJerome Jutteau2013-04-21 06:35:16 +0200
commit89180a5289dad3238563bd86377aa41944af756f (patch)
tree13075a4b054588646492bacebc868ca42d6c765e /digital/io-hub/src/apbirthday/candles.cc
parent5e550fa2a08633dd2276be4839d6373112573fd4 (diff)
digital/io-hub/src/apbirthday: add tests in proto and functions for candles
Diffstat (limited to 'digital/io-hub/src/apbirthday/candles.cc')
-rw-r--r--digital/io-hub/src/apbirthday/candles.cc103
1 files changed, 73 insertions, 30 deletions
diff --git a/digital/io-hub/src/apbirthday/candles.cc b/digital/io-hub/src/apbirthday/candles.cc
index 2e0f3661..c60281f4 100644
--- a/digital/io-hub/src/apbirthday/candles.cc
+++ b/digital/io-hub/src/apbirthday/candles.cc
@@ -96,43 +96,47 @@ inline bool Candles::is_near (int pos)
return pos >= far_count;
}
-
inline bool Candles::is_far (int pos)
{
return pos < far_count;
}
-void Candles::deploy_arm ()
+void Candles::flamby_arm ()
{
- // Deploy arm.
- robot->hardware.cake_arm_out.set (true);
- robot->hardware.cake_arm_in.set (false);
- // Prepare punhers.
- unpush_far ();
- unpush_near ();
-}
-
-void Candles::undeploy_arm_1 ()
-{
- // Get a flabby arm (usefull in case of failure).
robot->hardware.cake_arm_out.set (false);
robot->hardware.cake_arm_in.set (false);
- // Prepare far puncher to undeploy.
- Candles::push_far ();
- // Be sure the near punched in not punching.
- Candles::unpush_near ();
}
-void Candles::undeploy_arm_2 ()
+void Candles::flamby_far ()
{
- // Unleach far puncher.
robot->hardware.cake_push_far_out.set (false);
robot->hardware.cake_push_far_in.set (false);
- // Put arm back.
+}
+
+void Candles::crampe_arm ()
+{
+ robot->hardware.cake_arm_out.set (true);
+ robot->hardware.cake_arm_in.set (true);
+}
+
+void Candles::crampe_far ()
+{
+ robot->hardware.cake_push_far_out.set (true);
+ robot->hardware.cake_push_far_in.set (true);
+}
+
+void Candles::arm_back ()
+{
robot->hardware.cake_arm_out.set (false);
robot->hardware.cake_arm_in.set (true);
}
+void Candles::arm_out ()
+{
+ robot->hardware.cake_arm_out.set (true);
+ robot->hardware.cake_arm_in.set (false);
+}
+
void Candles::push_near ()
{
robot->hardware.cake_push_near_out.set (true);
@@ -157,15 +161,44 @@ void Candles::unpush_far ()
robot->hardware.cake_push_far_in.set (true);
}
+void Candles::deploy_arm ()
+{
+ // Deploy arm.
+ arm_out ();
+ // Prepare punhers.
+ unpush_far ();
+ unpush_near ();
+}
+
+void Candles::undeploy_arm_1 ()
+{
+ arm_back ();
+ // Just to be sure.
+ unpush_far ();
+ unpush_near ();
+}
+
+void Candles::undeploy_arm_2 ()
+{
+ crampe_far ();
+}
+
+void Candles::undeploy_arm_3 ()
+{
+ flamby_far ();
+}
+
// Global candle FSM.
FSM_STATES (AI_CANDLE_OFF,
AI_CANDLE_INIT,
AI_CANDLE_SLEEPING,
AI_CANDLE_DEPLOYING,
- AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED,
+ AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1,
+ AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2,
AI_CANDLE_READY,
AI_CANDLE_UNDEPLOYING,
- AI_CANDLE_UNDEPLOYING_2)
+ AI_CANDLE_UNDEPLOYING_2,
+ AI_CANDLE_UNDEPLOYING_3)
FSM_EVENTS (ai_candle_deploy,
ai_candle_undeploy,
@@ -180,7 +213,7 @@ FSM_TRANS (AI_CANDLE_OFF, init_actuators, AI_CANDLE_INIT)
Candles::deploy_arm ();
}
-FSM_TRANS_TIMEOUT (AI_CANDLE_INIT, 125, AI_CANDLE_UNDEPLOYING)
+FSM_TRANS_TIMEOUT (AI_CANDLE_INIT, 200, AI_CANDLE_UNDEPLOYING)
{
Candles::undeploy_arm_1 ();
}
@@ -195,9 +228,9 @@ FSM_TRANS (AI_CANDLE_SLEEPING, ai_candle_undeploy, AI_CANDLE_SLEEPING)
robot->fsm_queue.post (FSM_EVENT (ai_candle_success));
}
-FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING, 125,
+FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING, 200,
success, AI_CANDLE_READY,
- failure, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED)
+ failure, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1)
{
if (!robot->hardware.cake_arm_out_contact.get ())
{
@@ -213,11 +246,16 @@ FSM_TRANS_TIMEOUT (AI_CANDLE_DEPLOYING, 125,
}
}
-FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED, 12, AI_CANDLE_SLEEPING)
+FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_1, 160, AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2)
{
Candles::undeploy_arm_2 ();
}
+FSM_TRANS_TIMEOUT (AI_CANDLE_FALLING_BACK_TO_UNDEPLOYED_2, 56, AI_CANDLE_SLEEPING)
+{
+ Candles::undeploy_arm_3 ();
+}
+
FSM_TRANS (AI_CANDLE_READY, ai_candle_blow, AI_CANDLE_READY)
{
int i;
@@ -259,12 +297,17 @@ FSM_TRANS (AI_CANDLE_READY, ai_candle_undeploy, AI_CANDLE_UNDEPLOYING)
Candles::undeploy_arm_1 ();
}
-FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING, 10, AI_CANDLE_UNDEPLOYING_2) //TODO timeout value
+FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING, 160, AI_CANDLE_UNDEPLOYING_2)
{
Candles::undeploy_arm_2 ();
}
-FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_2, 125,
+FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_2, 56, AI_CANDLE_UNDEPLOYING_3)
+{
+ Candles::undeploy_arm_3 ();
+}
+
+FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING_3, 100,
success, AI_CANDLE_SLEEPING,
failure, AI_CANDLE_READY)
{
@@ -294,7 +337,7 @@ FSM_TRANS (AI_CANDLE_FAR_SLEEPING, ai_candle_far_punch, AI_CANDLE_FAR_PUNCHING)
Candles::push_far ();
}
-FSM_TRANS_TIMEOUT (AI_CANDLE_FAR_PUNCHING, 25, AI_CANDLE_FAR_SLEEPING) //TODO timeout value
+FSM_TRANS_TIMEOUT (AI_CANDLE_FAR_PUNCHING, 48, AI_CANDLE_FAR_SLEEPING)
{
Candles::unpush_far ();
}
@@ -312,7 +355,7 @@ FSM_TRANS (AI_CANDLE_NEAR_SLEEPING, ai_candle_near_punch, AI_CANDLE_NEAR_PUNCHIN
Candles::push_near ();
}
-FSM_TRANS_TIMEOUT (AI_CANDLE_NEAR_PUNCHING, 25, AI_CANDLE_NEAR_SLEEPING) //TODO timeout value
+FSM_TRANS_TIMEOUT (AI_CANDLE_NEAR_PUNCHING, 37, AI_CANDLE_NEAR_SLEEPING)
{
Candles::unpush_near ();
}