summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-09 12:08:58 +0200
committerNicolas Schodet2013-05-09 12:08:58 +0200
commit3335b166697b98ac899024930d497fa5811ae04d (patch)
treea29976e0429ef20d375fb9ba10fc40292453211e /digital
parent768871bf70ee1e6873f9420bee7c46777de4d4b0 (diff)
digital/io-hub/src/apbirthday: give up gifts if too far
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/top.cc12
1 files changed, 12 insertions, 0 deletions
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index 4cc99028..ba6b9d09 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -269,6 +269,11 @@ top_fsm_gen_event ()
if (ANGFSM_CAN_HANDLE (AI, top_gifts_open))
{
Position pos = robot->asserv.get_position ();
+ if (pos.v.y > pg_gifts_distance + BOT_SIZE_SIDE + 60)
+ {
+ if (ANGFSM_CAN_HANDLE (AI, top_gifts_blocked))
+ return true;
+ }
int arm_x = pos.v.x + bot_gift_arm_x;
for (int i = 0; i < Gifts::nb; i++)
{
@@ -364,6 +369,8 @@ ANGFSM_EVENTS (
top_plate_present,
// Open a gift now.
top_gifts_open,
+ // Problem with gifts.
+ top_gifts_blocked,
// Start candle demo.
top_demo_candles,
// Start candle arm demo.
@@ -671,6 +678,11 @@ FSM_TRANS (TOP_GIFTS_OPEN, top_gifts_open, TOP_GIFTS_OPEN)
top.gifts_opening = 250 / 4;
}
+FSM_TRANS (TOP_GIFTS_OPEN, top_gifts_blocked, TOP_DECISION)
+{
+ robot->move.stop ();
+}
+
///
/// Demo mode.
///