summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-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
-rw-r--r--host/simu/robots/apbirthday/model/bag.py2
-rw-r--r--host/simu/robots/apbirthday/view/bag.py4
5 files changed, 77 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);
+}
+
diff --git a/host/simu/robots/apbirthday/model/bag.py b/host/simu/robots/apbirthday/model/bag.py
index 768b1f4a..28295e63 100644
--- a/host/simu/robots/apbirthday/model/bag.py
+++ b/host/simu/robots/apbirthday/model/bag.py
@@ -39,6 +39,8 @@ class Bag:
self.color_switch.state = random.choice ((False, True))
self.color_switch.notify ()
self.jack = Switch (link_bag.raw_jack, invert = True)
+ self.strat_switch = Switch (link_bag.ihm_strat, invert = True)
+ self.robot_nb_switch = Switch (link_bag.ihm_robot_nb, invert = True)
self.beacon = RoundObstacle (40, 5)
table.obstacles.append (self.beacon)
self.position = Position (link_bag.asserv.position, [ self.beacon ])
diff --git a/host/simu/robots/apbirthday/view/bag.py b/host/simu/robots/apbirthday/view/bag.py
index 6a9e2d3d..d5c1744d 100644
--- a/host/simu/robots/apbirthday/view/bag.py
+++ b/host/simu/robots/apbirthday/view/bag.py
@@ -34,6 +34,10 @@ class Bag:
self.jack = Switch (sensor_frame, model_bag.jack, 'Jack')
self.color_switch = Switch (sensor_frame, model_bag.color_switch,
'Color')
+ self.strat_switch = Switch (sensor_frame, model_bag.strat_switch,
+ 'Strat')
+ self.robot_nb_switch = Switch (sensor_frame,
+ model_bag.robot_nb_switch, 'Nb robots')
self.robot = Robot (table, model_bag.position, model_bag.cake_arm)
self.distance_sensor = [DistanceSensorUS (self.robot, ds)
for ds in model_bag.distance_sensor]