From c6a8cc44fa11777a35ac329602b4a47b3145bf30 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Fri, 3 May 2013 18:34:50 +0200 Subject: digital/io-hub/src: move path stuff for apbirthday 2013 in class Path_2013 Class Path contains only the generic code to process generic obstacles, like the opponent robots. All the static obstacles of the 2013 playground, should be managed by class Path_2013. --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/path_2013.cc | 64 ++++++++++++++++++++++++++++++ digital/io-hub/src/apbirthday/path_2013.hh | 53 +++++++++++++++++++++++++ digital/io-hub/src/apbirthday/robot.hh | 4 +- digital/io-hub/src/common-cc/path.cc | 40 ++++++++----------- digital/io-hub/src/common-cc/path.hh | 52 ++++++++++-------------- 6 files changed, 158 insertions(+), 57 deletions(-) create mode 100644 digital/io-hub/src/apbirthday/path_2013.cc create mode 100644 digital/io-hub/src/apbirthday/path_2013.hh (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 105d230a..215c71bc 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 lcd.cc beacon.cc \ potentiometer.host.cc potentiometer.stm32.cc \ pressure.cc chrono.host.cc chrono.stm32.cc debounce.cc \ - radar.cc radar_2013.cc obstacles.cc path.cc strat.cc \ + radar.cc radar_2013.cc obstacles.cc path.cc path_2013.cc strat.cc \ outputs.cc \ top.cc init.cc move.cc candles.cc cannon.cc drinks.cc plate.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ diff --git a/digital/io-hub/src/apbirthday/path_2013.cc b/digital/io-hub/src/apbirthday/path_2013.cc new file mode 100644 index 00000000..1c5acdbe --- /dev/null +++ b/digital/io-hub/src/apbirthday/path_2013.cc @@ -0,0 +1,64 @@ +// path_2013.cc +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Olivier Lanneluc +// +// 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 "path_2013.hh" +#include "bot.hh" +#ifdef HOST +#include "debug.host.hh" +#endif + +Path_2013::Path_2013() : Path( (pg_border_distance + pg_plate_size_border*2 + BOT_SIZE_RADIUS/2), + (pg_border_distance), + (pg_width - pg_border_distance - pg_plate_size_border*2 - BOT_SIZE_RADIUS/2), + (pg_length - pg_border_distance)) +{ + host_debug("Path_2013 constructor\n"); + obstacles = new path_obstacle_t[PATH_2013_OBSTACLES_NB]; + navpoints = new vect_t[PATH_2013_NAVPOINTS_NB]; + navweights = new weight_t[PATH_2013_NAVPOINTS_NB]; + astar_nodes = new struct astar_node_t[PATH_2013_NAVPOINTS_NB]; +} + +Path_2013::~Path_2013() +{ + delete[] obstacles; + delete[] navpoints; + delete[] navweights; + delete[] astar_nodes; +} + +void Path_2013::reset() +{ + /* Reset the mobile obstacles */ + Path::reset(); + + /* Add the cake obstacle */ + add_obstacle( pg_cake_pos, + pg_cake_radius, + PATH_2013_CAKE_NAVPOINTS_NB * 2 /* only half the navpoints are on the playground */, + PATH_2013_CAKE_NAVPOINTS_LAYERS, + 0 /* no extra clearance radius */, + true /* it is allowed to target the center */); +} diff --git a/digital/io-hub/src/apbirthday/path_2013.hh b/digital/io-hub/src/apbirthday/path_2013.hh new file mode 100644 index 00000000..17392aad --- /dev/null +++ b/digital/io-hub/src/apbirthday/path_2013.hh @@ -0,0 +1,53 @@ +#ifndef path_2013_hh +#define path_2013_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Olivier Lanneluc +// +// 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 "path.hh" +#include "playground_2013.hh" + +/** 2013 path finding class */ +class Path_2013 : public Path +{ + public: + /** Constructor */ + Path_2013(); + /** Destructor */ + ~Path_2013(); + /** (overloaded) Reset path computation, remove mobile obstacles, add static obstacles */ + void reset(void); + + private: + /** Number of additional obstacles on the playground */ + static const int PATH_2013_OBSTACLES_NB = PATH_OBSTACLES_NB + 1/*cake*/; + /** Number of points for the cake (plus 1) */ + static const int PATH_2013_CAKE_NAVPOINTS_NB = 6; + /** Number of navigation points layers for the cake. */ + static const int PATH_2013_CAKE_NAVPOINTS_LAYERS = 1; + /** Number of additional navigation points. */ + static const int PATH_2013_NAVPOINTS_NB = + PATH_NAVPOINTS_NB + PATH_2013_CAKE_NAVPOINTS_LAYERS * PATH_2013_CAKE_NAVPOINTS_NB; +}; + +#endif // path_2013_hh diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 3483803e..a92b36ef 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -40,7 +40,7 @@ #include "radar_2013.hh" #include "obstacles.hh" #include "move.hh" -#include "path.hh" +#include "path_2013.hh" #include "strat.hh" #include "candles.hh" #include "drinks.hh" @@ -115,7 +115,7 @@ class Robot : public ucoo::Proto::Handler /// Obstacle database. Obstacles obstacles; /// Path finding. - Path path; + Path_2013 path; /// Move FSM. Move move; /// Strategy. diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 86c045d1..1340df64 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -27,7 +27,6 @@ #include "path.hh" #include "bot.hh" #include "robot.hh" -#include "playground.hh" #ifdef HOST #include "debug.host.hh" #endif @@ -63,11 +62,18 @@ static int32_t pos_dot_product(vect_t* pa, vect_t* pb, vect_t* pc, vect_t* pd) return vect_dot_product(&vab, &vcd); } -Path::Path() : - border_xmin(pg_border_distance + pg_plate_size_border*2 + BOT_SIZE_RADIUS/2), - border_ymin(pg_border_distance), - border_xmax(pg_width - pg_border_distance - pg_plate_size_border*2 - BOT_SIZE_RADIUS/2), - border_ymax(pg_length - pg_border_distance), +Path::Path( const uint16_t border_xmin, + const uint16_t border_ymin, + const uint16_t border_xmax, + const uint16_t border_ymax) : + border_xmin(border_xmin), + border_ymin(border_ymin), + border_xmax(border_xmax), + border_ymax(border_ymax), + obstacles(NULL), + navpoints(NULL), + navweights(NULL), + astar_nodes(NULL), escape_factor(0), obstacles_nb(0), navpoints_nb(0), @@ -79,8 +85,6 @@ Path::Path() : void Path::reset() { vect_t nul = {0,0}; - - /* Reset everything */ host_debug("Path reset\n"); obstacles_nb = 0; navpoints_nb = PATH_RESERVED_NAVPOINTS_NB; @@ -88,16 +92,6 @@ void Path::reset() navpoints[PATH_NAVPOINT_DST_IDX] = nul; next_node = 0; escape_factor = 0; - -#ifdef playground_2013_hh - /* Declare the cake as an obstacle */ - add_obstacle( pg_cake_pos, - pg_cake_radius, - PATH_CAKE_NAVPOINTS_NB * 2 /* only half the navpoints are on the playground */, - PATH_CAKE_NAVPOINTS_LAYERS, - 0 /* no extra clearance radius */, - true /*target the center is allowed*/); -#endif } void Path::add_obstacle( const vect_t &c, @@ -275,7 +269,7 @@ void Path::compute(weight_t escape) escape_factor = escape; /* Call the A* algorithm */ - path_found = (bool)astar(astar_nodes, PATH_NAVPOINTS_NB, PATH_NAVPOINT_DST_IDX, PATH_NAVPOINT_SRC_IDX); + path_found = (bool)astar(astar_nodes, navpoints_nb, PATH_NAVPOINT_DST_IDX, PATH_NAVPOINT_SRC_IDX); if (path_found) { /* Store next node to go to */ @@ -283,7 +277,7 @@ void Path::compute(weight_t escape) #ifdef PATH_DEBUG /* Log and display the path found */ - vect_t path[PATH_NAVPOINTS_NB]; + vect_t path[navpoints_nb]; int node = PATH_NAVPOINT_SRC_IDX; int path_nb = 0; @@ -330,7 +324,7 @@ bool Path::get_next(vect_t &p) { if (path_found) { - ucoo::assert(next_node