summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-07 20:26:16 +0200
committerNicolas Schodet2013-04-08 00:25:21 +0200
commitd2d7026f4103701ddf533994407229d7d7bdbb4b (patch)
treec74c8cd46405063044b556c3794481a989bfb0d0
parent84c1c892452971953c4a243ac8bcec33ba342988 (diff)
digital/io-hub/src/apbirthday: follow the cake, or leave
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc3
-rw-r--r--digital/io-hub/src/apbirthday/strat.hh4
-rw-r--r--digital/io-hub/src/apbirthday/top.cc67
3 files changed, 50 insertions, 24 deletions
diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc
index 95518d09..9c08f43d 100644
--- a/digital/io-hub/src/apbirthday/strat.cc
+++ b/digital/io-hub/src/apbirthday/strat.cc
@@ -31,7 +31,7 @@ Strat::decision (vect_t &pos)
return CANDLES;
}
-void
+bool
Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle)
{
// TODO: this is a stub.
@@ -45,6 +45,7 @@ Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle)
decision.direction = Asserv::FORWARD;
decision.end_angle = G_ANGLE_UF016_DEG (-180. / 6);
}
+ return true;
}
void
diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh
index 45737c65..1b4aa471 100644
--- a/digital/io-hub/src/apbirthday/strat.hh
+++ b/digital/io-hub/src/apbirthday/strat.hh
@@ -46,8 +46,8 @@ class Strat
public:
/// Return new decision and associated position.
Decision decision (vect_t &pos);
- /// Take a decision related to candles.
- void decision_candles (CandlesDecision &decision,
+ /// Take a decision related to candles, return false to give up candles.
+ bool decision_candles (CandlesDecision &decision,
uint16_t robot_angle);
/// Report a failure to apply the previous decision.
void failure ();
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index 05ef0a17..5f35ade1 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -31,6 +31,12 @@ extern "C" {
#include "angfsm.h"
}
+#ifdef TARGET_host
+typedef unsigned Branch;
+#else
+typedef angfsm_AI_branch_t Branch;
+#endif
+
#include <cmath>
/// Top context.
@@ -99,16 +105,37 @@ top_candle_for_angle (uint16_t a, Candles::Floor floor,
}
/// Start follow mode, ask Strat for what to do.
-static void
+static bool
top_follow_start ()
{
uint16_t robot_angle = top_cake_angle_robot ();
- robot->strat.decision_candles (top.candles, robot_angle);
- for (int floor = Candles::NEAR; floor < Candles::FLOOR_NB; floor++)
+ bool go_candle = robot->strat.decision_candles (top.candles, robot_angle);
+ if (go_candle)
{
- top.candles_last_blown[floor] =
- top_candle_for_angle (robot_angle, Candles::Floor (floor),
- top.candles.direction);
+ for (int floor = Candles::NEAR; floor < Candles::FLOOR_NB; floor++)
+ {
+ top.candles_last_blown[floor] =
+ top_candle_for_angle (robot_angle, Candles::Floor (floor),
+ top.candles.direction);
+ }
+ }
+ return go_candle;
+}
+
+/// Can follow cake, decide if this is a good thing, or leave.
+static Branch
+top_follow_or_leave ()
+{
+ if (top_follow_start ())
+ {
+ robot->asserv.follow (top.candles.direction);
+ return FSM_BRANCH (candles);
+ }
+ else
+ {
+ // TODO: take a smart decision to avoid collision.
+ robot->asserv.stop ();
+ return FSM_BRANCH (tangent);
}
}
@@ -236,10 +263,12 @@ FSM_TRANS (TOP_CANDLES_ENTER_DEPLOY, ai_candle_failure,
robot->asserv.move_distance (BOT_SIZE_RADIUS - BOT_SIZE_SIDE);
}
-FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_success, TOP_CANDLES_FOLLOW)
+FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_success,
+ tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE,
+ turn, TOP_CANDLES_LEAVE_TURN,
+ candles, TOP_CANDLES_FOLLOW)
{
- top_follow_start ();
- robot->asserv.follow (top.candles.direction);
+ return top_follow_or_leave ();
}
FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_failure, TOP_CANDLES_LEAVE_TURN)
@@ -250,29 +279,25 @@ FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_failure, TOP_CANDLES_LEAVE_TURN)
FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_finished,
tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE,
turn, TOP_CANDLES_LEAVE_TURN,
- reverse, TOP_CANDLES_FOLLOW)
+ candles, TOP_CANDLES_FOLLOW)
{
- // TODO: take a smart decision to avoid collision.
- robot->asserv.stop ();
- return FSM_BRANCH (tangent);
+ return top_follow_or_leave ();
}
FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_blocked,
tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE,
- turn, TOP_CANDLES_LEAVE_TURN)
+ turn, TOP_CANDLES_LEAVE_TURN,
+ candles, TOP_CANDLES_FOLLOW)
{
- // TODO: take a smart decision to avoid collision.
- robot->asserv.stop ();
- return FSM_BRANCH (tangent);
+ return top_follow_or_leave ();
}
FSM_TRANS (TOP_CANDLES_FOLLOW, robot_move_failure,
tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE,
- turn, TOP_CANDLES_LEAVE_TURN)
+ turn, TOP_CANDLES_LEAVE_TURN,
+ candles, TOP_CANDLES_FOLLOW)
{
- // TODO: take a smart decision to avoid collision.
- robot->asserv.stop ();
- return FSM_BRANCH (tangent);
+ return top_follow_or_leave ();
}
FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_success,