From 9a29545859c8513e48b813804b6fc1ea3ec3aa24 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 21 Apr 2013 18:42:49 +0200 Subject: digital/io-hub/src/apbirthday: add follow the cake demo mode --- digital/io-hub/src/apbirthday/robot.cc | 6 ++++++ digital/io-hub/src/apbirthday/top.cc | 26 ++++++++++++++++++++++++-- digital/io-hub/src/apbirthday/top.hh | 4 ++++ 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 76a26d7a..7137ea3e 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -218,6 +218,12 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size // Enter FSM debug mode, then step once. fsm_debug_state_ = FSM_DEBUG_STEP; break; + case c ('d', 2): + // Demo mode. + // 1B: 'f for follow mode. + // 1B: 00 for clockwise, 01 for anti-clockwise + top_demo_follow (args[0] == 1 ? 1 : -1); + break; case c ('m', 5): // Go to position. // 2H: x, y. diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index fa94f655..c96b8c12 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -224,6 +224,13 @@ top_fsm_gen_event () return false; } +void +top_demo_follow (int dir_sign) +{ + top.candles.dir_sign = dir_sign; + ANGFSM_HANDLE (AI, top_demo_follow); +} + ANGFSM_INIT ANGFSM_STATES ( @@ -253,7 +260,9 @@ ANGFSM_STATES ( // Demo mode: push the wall near the cake. TOP_DEMO_CANDLES_PUSH_WALL, // Demo mode: move away from the wall. - TOP_DEMO_CANDLES_MOVE_AWAY) + TOP_DEMO_CANDLES_MOVE_AWAY, + // Demo mode: follow the cake (or anything else actually). + TOP_DEMO_FOLLOW) ANGFSM_EVENTS ( // Cake following finished (end point reached). @@ -261,7 +270,9 @@ ANGFSM_EVENTS ( // Problem with cake following. top_follow_blocked, // Start candle demo. - top_demo_candles) + top_demo_candles, + // Start follow the cake demo. + top_demo_follow) ANGFSM_START_WITH (TOP_START) @@ -428,3 +439,14 @@ FSM_TRANS (TOP_INIT, top_demo_candles, TOP_CANDLES_GOTO_NORMAL) + pg_cake_distance + BOT_SIZE_SIDE); } +FSM_TRANS (TOP_START, top_demo_follow, TOP_DEMO_FOLLOW) +{ + robot->asserv.follow (top.candles.dir_sign == 1 + ? Asserv::FORWARD : Asserv::BACKWARD); +} + +FSM_TRANS (TOP_DEMO_FOLLOW, top_follow_finished, TOP_DEMO_FOLLOW) +{ + // Transition needed for top_update. +} + diff --git a/digital/io-hub/src/apbirthday/top.hh b/digital/io-hub/src/apbirthday/top.hh index 229c909b..09bb9ce0 100644 --- a/digital/io-hub/src/apbirthday/top.hh +++ b/digital/io-hub/src/apbirthday/top.hh @@ -43,4 +43,8 @@ top_update (); bool top_fsm_gen_event (); +/// Lauch follow the cake demo mode. +void +top_demo_follow (int dir_sign); + #endif // top_hh -- cgit v1.2.3