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/Makefile | 2 +- digital/io-hub/src/apbirthday/playground_2013.hh | 37 +++ digital/io-hub/src/apbirthday/robot.cc | 2 + digital/io-hub/src/apbirthday/robot.hh | 3 + digital/io-hub/src/apbirthday/strat.cc | 54 +++++ digital/io-hub/src/apbirthday/strat.hh | 56 +++++ digital/io-hub/src/apbirthday/top.cc | 279 ++++++++++++++++++++++- digital/io-hub/src/apbirthday/top.hh | 31 +++ 8 files changed, 461 insertions(+), 3 deletions(-) create mode 100644 digital/io-hub/src/apbirthday/playground_2013.hh create mode 100644 digital/io-hub/src/apbirthday/strat.cc create mode 100644 digital/io-hub/src/apbirthday/strat.hh create mode 100644 digital/io-hub/src/apbirthday/top.hh diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 9eacfcb1..968c563a 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -7,7 +7,7 @@ apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ i2c_queue.cc asserv.cc mimot.cc beacon.cc \ potentiometer.cc \ pressure.cc chrono.host.cc chrono.stm32.cc debounce.cc \ - radar.cc radar_2013.cc obstacles.cc path.cc \ + radar.cc radar_2013.cc obstacles.cc path.cc strat.cc \ outputs.cc \ top.cc init.cc move.cc candles.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ diff --git a/digital/io-hub/src/apbirthday/playground_2013.hh b/digital/io-hub/src/apbirthday/playground_2013.hh new file mode 100644 index 00000000..d0911ba4 --- /dev/null +++ b/digital/io-hub/src/apbirthday/playground_2013.hh @@ -0,0 +1,37 @@ +#ifndef playground_2013_hh +#define playground_2013_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "playground.hh" + +/// Largest radius of the cake. +static const int pg_cake_radius = 500; + +/// Cake center X and Y. +static const vect_t pg_cake_pos = { pg_width / 2, pg_length }; + +/// Distance from the cake to blow candles. +static const int pg_cake_distance = 30; + +#endif // playground_2013_hh diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 23c9665d..e86ce518 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -24,6 +24,7 @@ #include "robot.hh" #include "bot.hh" +#include "top.hh" #include "ucoolib/arch/arch.hh" #include "ucoolib/utils/bytes.hh" @@ -119,6 +120,7 @@ Robot::main_loop () pressure.update (); jack.update (); outputs_set_.update (); + top_update (); // Handle communications. bool sync = main_i2c_queue_.sync (); zb_i2c_queue_.sync (); diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index b17c0dae..5ad6418d 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -36,6 +36,7 @@ #include "obstacles.hh" #include "move.hh" #include "path.hh" +#include "strat.hh" #include "candles.hh" #include "ucoolib/base/proto/proto.hh" @@ -96,6 +97,8 @@ class Robot : public ucoo::Proto::Handler Path path; /// Move FSM. Move move; + /// Strategy. + Strat strat; /// Candles. Candles candles; private: diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc new file mode 100644 index 00000000..95518d09 --- /dev/null +++ b/digital/io-hub/src/apbirthday/strat.cc @@ -0,0 +1,54 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "strat.hh" + +Strat::Decision +Strat::decision (vect_t &pos) +{ + // TODO: this is a stub. + pos = pg_cake_pos; + return CANDLES; +} + +void +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.end_angle = G_ANGLE_UF016_DEG (180 + 180. / 6); + } + else + { + decision.direction = Asserv::FORWARD; + decision.end_angle = G_ANGLE_UF016_DEG (-180. / 6); + } +} + +void +Strat::failure () +{ +} + diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh new file mode 100644 index 00000000..45737c65 --- /dev/null +++ b/digital/io-hub/src/apbirthday/strat.hh @@ -0,0 +1,56 @@ +#ifndef strat_hh +#define strat_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "defs.hh" +#include "playground_2013.hh" +#include "asserv.hh" + +/// High level strategy decision making. +class Strat +{ + public: + enum Decision + { + CANDLES, + }; + /// Information on a candle decision. + struct CandlesDecision + { + /// Movement direction. + Asserv::DirectionConsign direction; + /// Angle relative to cake to end the movement. + uint16_t end_angle; + }; + public: + /// Return new decision and associated position. + Decision decision (vect_t &pos); + /// Take a decision related to candles. + void decision_candles (CandlesDecision &decision, + uint16_t robot_angle); + /// Report a failure to apply the previous decision. + void failure (); +}; + +#endif // strat_hh 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) +{ +} + diff --git a/digital/io-hub/src/apbirthday/top.hh b/digital/io-hub/src/apbirthday/top.hh new file mode 100644 index 00000000..95f20a44 --- /dev/null +++ b/digital/io-hub/src/apbirthday/top.hh @@ -0,0 +1,31 @@ +#ifndef top_hh +#define top_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +/// Update external consign when following cake. +void +top_update (); + +#endif // top_hh -- cgit v1.2.3