From bcaf3117b0e0c7c4dc8bf5442637d2639a92989a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 8 Apr 2013 22:03:33 +0200 Subject: digital/io-hub/src/apbirthday: detect obstacles when following the cake --- digital/io-hub/src/apbirthday/robot.cc | 3 ++ digital/io-hub/src/apbirthday/strat.cc | 4 +- digital/io-hub/src/apbirthday/strat.hh | 5 +-- digital/io-hub/src/apbirthday/top.cc | 69 ++++++++++++++++++++++------------ digital/io-hub/src/apbirthday/top.hh | 8 ++++ 5 files changed, 61 insertions(+), 28 deletions(-) diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index e86ce518..d6f29964 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -179,6 +179,9 @@ Robot::fsm_gen_event () if (ANGFSM_HANDLE_VAR (AI, event)) return true; } + // Top FSM events. + if (top_fsm_gen_event ()) + return true; // Check obstacles. if (move.check_obstacles ()) return true; diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc index 9c08f43d..e93eaeff 100644 --- a/digital/io-hub/src/apbirthday/strat.cc +++ b/digital/io-hub/src/apbirthday/strat.cc @@ -37,12 +37,12 @@ Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle) // TODO: this is a stub. if (robot_angle > G_ANGLE_UF016_DEG (-90)) { - decision.direction = Asserv::BACKWARD; + decision.dir_sign = -1; decision.end_angle = G_ANGLE_UF016_DEG (180 + 180. / 6); } else { - decision.direction = Asserv::FORWARD; + decision.dir_sign = 1; decision.end_angle = G_ANGLE_UF016_DEG (-180. / 6); } return true; diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh index 1b4aa471..fb1a8bf3 100644 --- a/digital/io-hub/src/apbirthday/strat.hh +++ b/digital/io-hub/src/apbirthday/strat.hh @@ -25,7 +25,6 @@ // }}} #include "defs.hh" #include "playground_2013.hh" -#include "asserv.hh" /// High level strategy decision making. class Strat @@ -38,8 +37,8 @@ class Strat /// Information on a candle decision. struct CandlesDecision { - /// Movement direction. - Asserv::DirectionConsign direction; + /// Movement direction, 1 (trigo) or -1 (antitrigo). + int dir_sign; /// Angle relative to cake to end the movement. uint16_t end_angle; }; diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 5f35ade1..46c55a29 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -72,8 +72,7 @@ top_cake_angle_robot () /// Compute the candle to blow for a given angle, when going in the given /// direction. static int -top_candle_for_angle (uint16_t a, Candles::Floor floor, - Asserv::DirectionConsign direction) +top_candle_for_angle (uint16_t a, Candles::Floor floor, int dir_sign) { /// Information for each floor. struct FloorInfo @@ -99,7 +98,7 @@ top_candle_for_angle (uint16_t a, Candles::Floor floor, const FloorInfo &i = floor_info[floor]; int index = i.first_index + (a - i.first_angle) / i.inter_angle; // For backward, add 1. - if (direction == Asserv::BACKWARD) + if (dir_sign == -1) index++; return index; } @@ -116,7 +115,7 @@ top_follow_start () { top.candles_last_blown[floor] = top_candle_for_angle (robot_angle, Candles::Floor (floor), - top.candles.direction); + top.candles.dir_sign); } } return go_candle; @@ -128,7 +127,8 @@ top_follow_or_leave () { if (top_follow_start ()) { - robot->asserv.follow (top.candles.direction); + robot->asserv.follow (top.candles.dir_sign == 1 + ? Asserv::FORWARD : Asserv::BACKWARD); return FSM_BRANCH (candles); } else @@ -139,47 +139,70 @@ top_follow_or_leave () } } +bool +top_follow_blocking (int dir_sign) +{ + Position robot_pos; + robot->asserv.get_position (robot_pos); + uint16_t robot_angle = top_cake_angle (robot_pos.v); + // Check for an obstacle on a small segment. + vect_t dst; + uint16_t dst_angle = robot_angle + dir_sign * G_ANGLE_UF016_DEG (30); + vect_from_polar_uf016 (&dst, pg_cake_radius + pg_cake_distance + + BOT_SIZE_SIDE, dst_angle); + vect_translate (&dst, &pg_cake_pos); + return robot->obstacles.blocking (robot_pos.v, dst, 200); +} + void top_update () { if (FSM_CAN_HANDLE (AI, top_follow_finished)) { - uint16_t robot_angle = top_cake_angle_robot (); // Update consign. int cons; const int k = 200; const int front_offset = 0x07fb; const int back_offset = 0x09af; - if (top.candles.direction == Asserv::FORWARD) + if (top.candles.dir_sign == 1) cons = - robot->hardware.adc_cake_front.read () + front_offset; else cons = robot->hardware.adc_cake_back.read () - back_offset; robot->asserv.follow_update (cons * k / 1000); + } +} + +bool +top_fsm_gen_event () +{ + if (ANGFSM_CAN_HANDLE (AI, top_follow_finished)) + { + uint16_t robot_angle = top_cake_angle_robot (); + int dir_sign = top.candles.dir_sign; // Check for movement end. - if ((top.candles.direction == Asserv::FORWARD - && robot_angle > top.candles.end_angle) - || (top.candles.direction == Asserv::BACKWARD - && robot_angle < top.candles.end_angle)) + if ((robot_angle - top.candles.end_angle) * dir_sign > 0) { - robot->fsm_queue.post (FSM_EVENT (top_follow_finished)); - return; + if (ANGFSM_HANDLE (AI, top_follow_finished)) + return true; } + // Check for obstacle. + if (top_follow_blocking (dir_sign)) + if (ANGFSM_HANDLE (AI, top_follow_blocked)) + return true; // Check for a candle to blow. - else + for (int floor = Candles::NEAR; floor < Candles::FLOOR_NB; floor++) { - for (int floor = Candles::NEAR; floor < Candles::FLOOR_NB; floor++) + int candle = top_candle_for_angle (robot_angle, + Candles::Floor (floor), + top.candles.dir_sign); + if (candle != top.candles_last_blown[floor]) { - int candle = top_candle_for_angle (robot_angle, - Candles::Floor (floor), - top.candles.direction); - if (candle != top.candles_last_blown[floor]) - { - robot->candles.blow (candle); - top.candles_last_blown[floor] = candle; - } + robot->candles.blow (candle); + top.candles_last_blown[floor] = candle; } } } + return false; } ANGFSM_INIT diff --git a/digital/io-hub/src/apbirthday/top.hh b/digital/io-hub/src/apbirthday/top.hh index 95f20a44..09a1b365 100644 --- a/digital/io-hub/src/apbirthday/top.hh +++ b/digital/io-hub/src/apbirthday/top.hh @@ -24,8 +24,16 @@ // // }}} +/// Test whether there is an obstacle blocking in the given direction. +bool +top_follow_blocking (int dir_sign); + /// Update external consign when following cake. void top_update (); +/// Generate TOP FSM events, return true if one event handled. +bool +top_fsm_gen_event (); + #endif // top_hh -- cgit v1.2.3