summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-21 18:42:49 +0200
committerNicolas Schodet2013-04-21 22:00:08 +0200
commit9a29545859c8513e48b813804b6fc1ea3ec3aa24 (patch)
treed6219fecd527d9d53ee0f3c606f462d49b05e55c /digital
parent64f61347d913f05fc81f08f169e402a3205afd6e (diff)
digital/io-hub/src/apbirthday: add follow the cake demo mode
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc6
-rw-r--r--digital/io-hub/src/apbirthday/top.cc26
-rw-r--r--digital/io-hub/src/apbirthday/top.hh4
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