From 3335b166697b98ac899024930d497fa5811ae04d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 9 May 2013 12:08:58 +0200 Subject: digital/io-hub/src/apbirthday: give up gifts if too far --- digital/io-hub/src/apbirthday/top.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'digital/io-hub') 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. /// -- cgit v1.2.3