summaryrefslogtreecommitdiff
path: root/digital/io-hub
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc12
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh4
-rw-r--r--digital/io-hub/src/apbirthday/top.cc59
3 files changed, 71 insertions, 4 deletions
diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc
index 8ed99782..36629698 100644
--- a/digital/io-hub/src/apbirthday/robot.cc
+++ b/digital/io-hub/src/apbirthday/robot.cc
@@ -43,6 +43,7 @@ Robot::Robot ()
chrono (90000 - 1000),
pressure (hardware.adc_pressure, hardware.pneum_open, mimot.motor0),
jack (hardware.raw_jack, 50),
+ demo (false),
usdist_control_ (2),
usdist0_ (usdist_control_, hardware.adc_dist0, hardware.dist0_sync, 100, 700, 650),
usdist1_ (usdist_control_, hardware.adc_dist1, hardware.dist1_sync, 100, 700, 650),
@@ -171,6 +172,9 @@ Robot::fsm_gen_event ()
fsm_handle_and_return (jack_inserted);
else
fsm_handle_and_return (jack_removed);
+ // Demo mode.
+ if (demo && demo_fsm_gen_event ())
+ return true;
// FSM queue.
if (fsm_queue.poll ())
{
@@ -187,6 +191,14 @@ Robot::fsm_gen_event ()
return false;
}
+bool
+Robot::demo_fsm_gen_event ()
+{
+ if (robot->hardware.ihm_strat.get ())
+ fsm_handle_and_return (top_demo_candles);
+ return false;
+}
+
void
Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size)
{
diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh
index 30dabc90..556e29eb 100644
--- a/digital/io-hub/src/apbirthday/robot.hh
+++ b/digital/io-hub/src/apbirthday/robot.hh
@@ -54,6 +54,8 @@ class Robot : public ucoo::Proto::Handler
void main_loop ();
/// Generate events for the FSM.
bool fsm_gen_event ();
+ /// Generate events for the FSM for demo mode.
+ bool demo_fsm_gen_event ();
/// Receive proto messages.
void proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size);
/// Send stats.
@@ -85,6 +87,8 @@ class Robot : public ucoo::Proto::Handler
Pressure pressure;
/// Jack debouncing.
Debounce jack;
+ /// Demo mode flag.
+ bool demo;
private:
/// US distance sensors controller.
ucoo::UsDistControl usdist_control_;
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index 71c568ea..77b99c6f 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -224,6 +224,8 @@ ANGFSM_INIT
ANGFSM_STATES (
// Initial state.
TOP_START,
+ // Initialising actuators.
+ TOP_INIT_ACTUATORS,
// Init done, waiting for rount start.
TOP_INIT,
// Decision state, one stop, one cycle.
@@ -242,17 +244,29 @@ ANGFSM_STATES (
// Candles: turn to leave, undeploy arm as soon as possible.
TOP_CANDLES_LEAVE_TURN,
// Candles: go away so that the robot is free to turn.
- TOP_CANDLES_LEAVE_GO_AWAY)
+ TOP_CANDLES_LEAVE_GO_AWAY,
+ // 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)
ANGFSM_EVENTS (
// Cake following finished (end point reached).
top_follow_finished,
// Problem with cake following.
- top_follow_blocked)
+ top_follow_blocked,
+ // Start candle demo.
+ top_demo_candles)
ANGFSM_START_WITH (TOP_START)
-FSM_TRANS (TOP_START, init_done, TOP_INIT)
+FSM_TRANS (TOP_START, init_actuators, TOP_INIT_ACTUATORS)
+{
+ // TODO: make sure the operator do not forget this is demo mode!
+ robot->demo = !robot->hardware.ihm_strat.get ();
+}
+
+FSM_TRANS (TOP_INIT_ACTUATORS, init_done, TOP_INIT)
{
// Color dependent init can go here.
}
@@ -263,8 +277,10 @@ FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
candles, TOP_CANDLES_GOTO_NORMAL,
- none, TOP_INIT)
+ none, TOP_START)
{
+ if (robot->demo)
+ return FSM_BRANCH (none);
vect_t d_pos;
Strat::Decision d = robot->strat.decision (d_pos);
switch (d)
@@ -278,6 +294,10 @@ FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
}
}
+///
+/// Candles.
+///
+
FSM_TRANS (TOP_CANDLES_GOTO_NORMAL, move_success, TOP_CANDLES_ENTER_DEPLOY)
{
ANGFSM_HANDLE (AI, ai_candle_deploy);
@@ -372,3 +392,34 @@ FSM_TRANS (TOP_CANDLES_LEAVE_GO_AWAY, robot_move_failure, TOP_DECISION)
{
}
+///
+/// Demo mode.
+///
+
+FSM_TRANS (TOP_INIT_ACTUATORS, top_demo_candles, TOP_DEMO_CANDLES_PUSH_WALL)
+{
+ team_color = TEAM_COLOR_LEFT;
+ robot->asserv.push_wall (Asserv::FORWARD, pg_cake_pos.x - pg_cake_radius
+ - 20 - BOT_SIZE_SIDE,
+ pg_length - BOT_FRONT_CONTACT_DIST,
+ G_ANGLE_UF016_DEG (90));
+}
+
+FSM_TRANS (TOP_DEMO_CANDLES_PUSH_WALL, robot_move_success,
+ TOP_DEMO_CANDLES_MOVE_AWAY)
+{
+ robot->asserv.move_distance (-200);
+}
+
+FSM_TRANS (TOP_DEMO_CANDLES_MOVE_AWAY, robot_move_success, TOP_CANDLES_GOTO_NORMAL)
+{
+ robot->move.start (pg_cake_pos, Asserv::BACKWARD, pg_cake_radius
+ + pg_cake_distance + BOT_SIZE_SIDE);
+}
+
+FSM_TRANS (TOP_INIT, top_demo_candles, TOP_CANDLES_GOTO_NORMAL)
+{
+ robot->move.start (pg_cake_pos, Asserv::BACKWARD, pg_cake_radius
+ + pg_cake_distance + BOT_SIZE_SIDE);
+}
+