summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-10 17:53:36 +0200
committerNicolas Schodet2013-05-10 17:53:36 +0200
commitd4d0cbc0a317a62347016a802ba26d55158204cf (patch)
tree2d66e4cf2c230c89ad57ebea13cc8884e8d1f905 /digital/io-hub
parent8fb69c363f745129896d211701a16d7ef8f3e82e (diff)
digital/io-hub/src/apbirthday: wait before turn when leaving the cake on problem
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/apbirthday/top.cc24
1 files changed, 16 insertions, 8 deletions
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index 59d2b0c8..8c401014 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -121,7 +121,7 @@ top_follow_start ()
/// Can follow cake, decide if this is a good thing, or leave.
static Branch
-top_follow_or_leave ()
+top_follow_or_leave (bool wait_before_turn)
{
if (top_follow_start ())
{
@@ -161,7 +161,8 @@ top_follow_or_leave ()
}
else
{
- robot->asserv.goto_angle (robot_angle);
+ if (!wait_before_turn)
+ robot->asserv.goto_angle (robot_angle);
return FSM_BRANCH (turn);
}
}
@@ -322,6 +323,8 @@ ANGFSM_STATES (
TOP_CANDLES_FOLLOW,
// Candles: tangent move to escape from an obstacle.
TOP_CANDLES_LEAVE_TANGENT_MOVE,
+ // Candles: wait a little bit before turning.
+ TOP_CANDLES_LEAVE_TURN_WAIT,
// Candles: turn to leave, undeploy arm as soon as possible.
TOP_CANDLES_LEAVE_TURN,
// Candles: wait for slow undeploying.
@@ -487,7 +490,7 @@ FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_success,
turn, TOP_CANDLES_LEAVE_TURN,
candles, TOP_CANDLES_FOLLOW)
{
- return top_follow_or_leave ();
+ return top_follow_or_leave (false);
}
FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_failure, TOP_CANDLES_LEAVE_TURN)
@@ -500,23 +503,23 @@ FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_finished,
turn, TOP_CANDLES_LEAVE_TURN,
candles, TOP_CANDLES_FOLLOW)
{
- return top_follow_or_leave ();
+ return top_follow_or_leave (false);
}
FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_blocked,
tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE,
- turn, TOP_CANDLES_LEAVE_TURN,
+ turn, TOP_CANDLES_LEAVE_TURN_WAIT,
candles, TOP_CANDLES_FOLLOW)
{
- return top_follow_or_leave ();
+ return top_follow_or_leave (true);
}
FSM_TRANS (TOP_CANDLES_FOLLOW, robot_move_failure,
tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE,
- turn, TOP_CANDLES_LEAVE_TURN,
+ turn, TOP_CANDLES_LEAVE_TURN_WAIT,
candles, TOP_CANDLES_FOLLOW)
{
- return top_follow_or_leave ();
+ return top_follow_or_leave (true);
}
FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_success,
@@ -531,6 +534,11 @@ FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_failure,
robot->asserv.goto_angle (top_cake_angle_robot ());
}
+FSM_TRANS_TIMEOUT (TOP_CANDLES_LEAVE_TURN_WAIT, 125, TOP_CANDLES_LEAVE_TURN)
+{
+ robot->asserv.goto_angle (top_cake_angle_robot ());
+}
+
FSM_TRANS (TOP_CANDLES_LEAVE_TURN, robot_move_success,
TOP_CANDLES_LEAVE_UNDEPLOY)
{