From 4654f5ce8140b371f5401ae40f90c2446e4d432e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 7 Apr 2013 10:31:24 +0200 Subject: digital/io-hub/src/apbirthday: first TOP FSM action, work in progress --- digital/io-hub/src/apbirthday/top.cc | 279 ++++++++++++++++++++++++++++++++++- 1 file changed, 277 insertions(+), 2 deletions(-) (limited to 'digital/io-hub/src/apbirthday/top.cc') diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 8975f67f..05ef0a17 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -21,17 +21,292 @@ // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // // }}} -//#include "top.hh" +#include "top.hh" +#include "robot.hh" +#include "playground_2013.hh" +#include "bot.hh" extern "C" { #define ANGFSM_NAME AI #include "angfsm.h" } +#include + +/// Top context. +struct top_t +{ + /// Candles decision information. + Strat::CandlesDecision candles; + /// Last blown candles. + int candles_last_blown[Candles::FLOOR_NB]; +}; +static top_t top; + +/// Compute the angle of the normal to the cake at the given point. +static uint16_t +top_cake_angle (const vect_t &pos) +{ + float dx = pos.x - pg_cake_pos.x; + float dy = pos.y - pg_cake_pos.y; + float angle_rad = std::atan2 (dy, dx); + uint16_t angle = angle_rad * ((1 << 16) / (2 * M_PI)); + return angle; +} + +/// Compute the angle of the normal to the cake at the robot position. +static uint16_t +top_cake_angle_robot () +{ + Position pos; + robot->asserv.get_position (pos); + return top_cake_angle (pos.v); +} + +/// 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) +{ + /// Information for each floor. + struct FloorInfo + { + /// Angle of the first candle, also include angle offset due + /// to arm position. + uint16_t first_angle; + /// Index of the first candle. + int first_index; + /// Angle between candles. + uint16_t inter_angle; + }; + static const FloorInfo floor_info[Candles::FLOOR_NB] = + { +#define NEAR_ARM_OFFSET_DEG 0 +#define FAR_ARM_OFFSET_DEG 9 + { G_ANGLE_UF016_DEG (180 + 180. / (12 * 2) - NEAR_ARM_OFFSET_DEG), 8, + G_ANGLE_UF016_DEG (180. / 12) }, + { G_ANGLE_UF016_DEG (180 + 180. / (8 * 2) - FAR_ARM_OFFSET_DEG), 0, + G_ANGLE_UF016_DEG (180. / 8) }, + }; + // Compute for the forward direction. + 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) + index++; + return index; +} + +/// Start follow mode, ask Strat for what to do. +static void +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++) + { + top.candles_last_blown[floor] = + top_candle_for_angle (robot_angle, Candles::Floor (floor), + top.candles.direction); + } +} + +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) + 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); + // 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)) + { + robot->fsm_queue.post (FSM_EVENT (top_follow_finished)); + return; + } + // Check for a candle to blow. + else + { + for (int floor = Candles::NEAR; floor < Candles::FLOOR_NB; 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; + } + } + } + } +} + ANGFSM_INIT ANGFSM_STATES ( // Initial state. - TOP_START) + TOP_START, + // Init done, waiting for rount start. + TOP_INIT, + // Decision state, one stop, one cycle. + TOP_DECISION, + // Candles: go to cake, with a normal move. + TOP_CANDLES_GOTO_NORMAL, + // Candles: deploy arm. + TOP_CANDLES_ENTER_DEPLOY, + // Candles: turn to be in position to follow the cake. + TOP_CANDLES_ENTER_TURN, + // Candles: follow the cake curve until destination angle, blowing + // candles on the way. + TOP_CANDLES_FOLLOW, + // Candles: tangent move to escape from an obstacle. + TOP_CANDLES_LEAVE_TANGENT_MOVE, + // 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) + +ANGFSM_EVENTS ( + // Cake following finished (end point reached). + top_follow_finished, + // Problem with cake following. + top_follow_blocked) ANGFSM_START_WITH (TOP_START) + +FSM_TRANS (TOP_START, init_done, TOP_INIT) +{ + // Color dependent init can go here. +} + +FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION) +{ +} + +FSM_TRANS_TIMEOUT (TOP_DECISION, 1, + candles, TOP_CANDLES_GOTO_NORMAL, + none, TOP_INIT) +{ + vect_t d_pos; + Strat::Decision d = robot->strat.decision (d_pos); + switch (d) + { + case Strat::CANDLES: + robot->move.start (d_pos, Asserv::BACKWARD, pg_cake_radius + + pg_cake_distance + BOT_SIZE_SIDE); + return FSM_BRANCH (candles); + default: + ucoo::assert_unreachable (); + } +} + +FSM_TRANS (TOP_CANDLES_GOTO_NORMAL, move_success, TOP_CANDLES_ENTER_DEPLOY) +{ + ANGFSM_HANDLE (AI, ai_candle_deploy); +} + +FSM_TRANS (TOP_CANDLES_GOTO_NORMAL, move_failure, TOP_DECISION) +{ + robot->strat.failure (); +} + +FSM_TRANS (TOP_CANDLES_ENTER_DEPLOY, ai_candle_success, TOP_CANDLES_ENTER_TURN) +{ + robot->asserv.goto_angle (top_cake_angle_robot () + + G_ANGLE_UF016_DEG (90)); +} + +FSM_TRANS (TOP_CANDLES_ENTER_DEPLOY, ai_candle_failure, + TOP_CANDLES_LEAVE_GO_AWAY) +{ + robot->asserv.move_distance (BOT_SIZE_RADIUS - BOT_SIZE_SIDE); +} + +FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_success, TOP_CANDLES_FOLLOW) +{ + top_follow_start (); + robot->asserv.follow (top.candles.direction); +} + +FSM_TRANS (TOP_CANDLES_ENTER_TURN, robot_move_failure, TOP_CANDLES_LEAVE_TURN) +{ + robot->asserv.goto_angle (top_cake_angle_robot ()); +} + +FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_finished, + tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE, + turn, TOP_CANDLES_LEAVE_TURN, + reverse, TOP_CANDLES_FOLLOW) +{ + // TODO: take a smart decision to avoid collision. + robot->asserv.stop (); + return FSM_BRANCH (tangent); +} + +FSM_TRANS (TOP_CANDLES_FOLLOW, top_follow_blocked, + tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE, + turn, TOP_CANDLES_LEAVE_TURN) +{ + // TODO: take a smart decision to avoid collision. + robot->asserv.stop (); + return FSM_BRANCH (tangent); +} + +FSM_TRANS (TOP_CANDLES_FOLLOW, robot_move_failure, + tangent, TOP_CANDLES_LEAVE_TANGENT_MOVE, + turn, TOP_CANDLES_LEAVE_TURN) +{ + // TODO: take a smart decision to avoid collision. + robot->asserv.stop (); + return FSM_BRANCH (tangent); +} + +FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_success, + TOP_CANDLES_LEAVE_TURN) +{ + robot->asserv.goto_angle (top_cake_angle_robot ()); +} + +FSM_TRANS (TOP_CANDLES_LEAVE_TANGENT_MOVE, robot_move_failure, + TOP_CANDLES_LEAVE_TURN) +{ + robot->asserv.goto_angle (top_cake_angle_robot ()); +} + +FSM_TRANS (TOP_CANDLES_LEAVE_TURN, robot_move_success, + TOP_CANDLES_LEAVE_GO_AWAY) +{ + // TODO: undeploy earlier, by computing arm end position. + ANGFSM_HANDLE (AI, ai_candle_undeploy); + robot->asserv.move_distance (BOT_SIZE_RADIUS - BOT_SIZE_SIDE); +} + +FSM_TRANS (TOP_CANDLES_LEAVE_TURN, robot_move_failure, + TOP_CANDLES_LEAVE_GO_AWAY) +{ + ANGFSM_HANDLE (AI, ai_candle_undeploy); + robot->asserv.move_distance (BOT_SIZE_RADIUS - BOT_SIZE_SIDE); +} + +FSM_TRANS (TOP_CANDLES_LEAVE_GO_AWAY, robot_move_success, TOP_DECISION) +{ +} + +FSM_TRANS (TOP_CANDLES_LEAVE_GO_AWAY, robot_move_failure, TOP_DECISION) +{ +} + -- cgit v1.2.3