From d2d7026f4103701ddf533994407229d7d7bdbb4b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 7 Apr 2013 20:26:16 +0200 Subject: digital/io-hub/src/apbirthday: follow the cake, or leave --- digital/io-hub/src/apbirthday/strat.cc | 3 +- digital/io-hub/src/apbirthday/strat.hh | 4 +- digital/io-hub/src/apbirthday/top.cc | 67 +++++++++++++++++++++++----------- 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 /// 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, -- cgit v1.2.3