summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-09 10:49:11 +0200
committerNicolas Schodet2013-05-09 10:49:11 +0200
commit7476a2dc131d4914e0bc7c17e844406e23e33814 (patch)
tree8e1b28dfd19d20a4ff86d28e834013938a65d837
parentae9606852e8ca6b967e94bb1d885be185dfe80ce (diff)
digital/io-hub/src/apbirthday: detect when cake following is failing
-rw-r--r--digital/io-hub/src/apbirthday/top.cc10
1 files changed, 9 insertions, 1 deletions
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index b325d54e..b22dc21d 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -36,6 +36,8 @@ struct top_t
Strat::CandlesDecision candles;
/// Last blown candles.
int candles_last_blown[Candles::FLOOR_NB];
+ /// Wow, we are too far or too near the cake!
+ int candles_too_far;
/// Plate decision information.
Strat::PlateDecision plate;
/// Gifts decision information.
@@ -123,6 +125,7 @@ top_follow_or_leave ()
{
robot->asserv.follow (top.candles.dir_sign == 1
? Asserv::FORWARD : Asserv::BACKWARD);
+ top.candles_too_far = 0;
return FSM_BRANCH (candles);
}
else
@@ -196,6 +199,10 @@ top_update ()
else
cons = robot->hardware.adc_cake_back.read () - back_offset;
robot->asserv.follow_update (cons * k / 1000);
+ if (std::abs (cons) > 0x400)
+ top.candles_too_far++;
+ else
+ top.candles_too_far = 0;
}
if (top.gifts_opening)
{
@@ -222,7 +229,8 @@ top_fsm_gen_event ()
return true;
}
// Check for obstacle.
- if (top_follow_blocking (dir_sign))
+ if (top_follow_blocking (dir_sign)
+ || top.candles_too_far > 125)
if (ANGFSM_HANDLE (AI, top_follow_blocked))
return true;
// Check for a candle to blow.