summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-05-03 18:34:50 +0200
committerNicolas Schodet2013-05-03 23:07:15 +0200
commitc6a8cc44fa11777a35ac329602b4a47b3145bf30 (patch)
tree6b4bffba590c3fe156e11fafef708b3d95b9a00f
parent80c332e283f7eaf02f8f2f9da792a30c8bf18f2b (diff)
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.
-rw-r--r--digital/io-hub/src/apbirthday/Makefile2
-rw-r--r--digital/io-hub/src/apbirthday/path_2013.cc64
-rw-r--r--digital/io-hub/src/apbirthday/path_2013.hh53
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh4
-rw-r--r--digital/io-hub/src/common-cc/path.cc40
-rw-r--r--digital/io-hub/src/common-cc/path.hh52
6 files changed, 158 insertions, 57 deletions
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<PATH_NAVPOINTS_NB);
+ ucoo::assert(next_node<navpoints_nb);
p = navpoints[next_node];
next_node = astar_nodes[next_node].prev;
}
@@ -357,12 +351,12 @@ void Path::prepare_score(const vect_t &src, weight_t escape)
{
host_debug("Path prepare score from src=(%u;%u) escape=%u\n", src.x, src.y, escape);
escape_factor = escape;
- astar_dijkstra_prepare(astar_nodes, PATH_NAVPOINTS_NB, get_point_index(src), PATH_NAVPOINT_DST_IDX);
+ astar_dijkstra_prepare(astar_nodes, navpoints_nb, get_point_index(src), PATH_NAVPOINT_DST_IDX);
}
weight_t Path::get_score(const vect_t &dst)
{
- uint16_t score = astar_dijkstra_finish(astar_nodes, PATH_NAVPOINTS_NB, get_point_index(dst));
+ uint16_t score = astar_dijkstra_finish(astar_nodes, navpoints_nb, get_point_index(dst));
host_debug("Path get score=%u for dst=(%u;%u)\n", score, dst.x, dst.y);
return score;
}
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index beb3a3e5..d673d0cf 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -25,8 +25,7 @@
//
// }}}
#include "defs.hh"
-#include "playground_2013.hh"
-
+#include "playground.hh"
extern "C" {
#include "modules/path/astar/astar.h"
}
@@ -58,9 +57,9 @@ class Path
{
public:
/** Initialise path */
- Path();
- /** Reset path computation, remove every obstacles */
- void reset(void);
+ Path(const uint16_t, const uint16_t, const uint16_t, const uint16_t);
+ /** Reset path computation, remove mobile obstacles, add static obstacles */
+ virtual void reset(void);
/** Set a moving obstacle position, radius and factor*/
void obstacle(const int index, const vect_t &c, const uint16_t r, const int f = 0, const bool target = false);
/** Set path source and destination */
@@ -82,28 +81,30 @@ class Path
(also reuse previously given escape factor) */
weight_t get_score(const vect_t &dst);
- private:
+ protected:
/** Add an obstacle on the field */
void add_obstacle(const vect_t &c, uint16_t r, int nodes, const int nlayers, const uint16_t clearance, const bool target);
-
- /** Number of possible obstacles. */
- static const int PATH_OBSTACLES_NB = (4+1/*cake*/);
+ /** Number of possible mobile obstacles. */
+ static const int PATH_OBSTACLES_NB = 4;
/** Number of points per standard obstacle. */
static const int PATH_OBSTACLES_NAVPOINTS_NB = 10;
-#ifdef playground_2013_hh
- /** Number of points for the cake (plus 1) */
- static const int PATH_CAKE_NAVPOINTS_NB = 6;
-#endif
- /** Number of navigation points layers for the cake. */
- static const int PATH_CAKE_NAVPOINTS_LAYERS = 1;
/** Number of navigation points layers for each obstacle. */
static const int PATH_OBSTACLES_NAVPOINTS_LAYERS = 2;
/** Number of navigation points. */
- static const int PATH_NAVPOINTS_NB = (PATH_RESERVED_NAVPOINTS_NB +
-#ifdef playground_2013_hh
- PATH_CAKE_NAVPOINTS_LAYERS * (PATH_CAKE_NAVPOINTS_NB - PATH_OBSTACLES_NAVPOINTS_NB) +
-#endif
- PATH_OBSTACLES_NAVPOINTS_LAYERS * (PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB));
+ static const int PATH_NAVPOINTS_NB =
+ (PATH_RESERVED_NAVPOINTS_NB + PATH_OBSTACLES_NAVPOINTS_LAYERS * PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB);
+ /** Borders, any point outside borders is eliminated. */
+ const uint16_t border_xmin, border_ymin, border_xmax, border_ymax;
+ /** List of obstacles. */
+ path_obstacle_t* obstacles;
+ /** List of navigation points coordonates */
+ vect_t* navpoints;
+ /** List of navigation points weights */
+ weight_t* navweights;
+ /** List of nodes used for A*. */
+ struct astar_node_t* astar_nodes;
+
+ private:
/** Navigation points weight precision (2^-n).
* Pay attention to overflow on weight_t variables */
static const int PATH_WEIGHT_PRECISION = 4;
@@ -115,23 +116,12 @@ class Path
/** Extra clearance area added to the radius of the navigation points
* circle to move the navpoints away from the obstacle circle */
static const uint16_t PATH_NAVPOINTS_CLEARANCE = 40;
-
- /** Borders, any point outside borders is eliminated. */
- const uint16_t border_xmin, border_ymin, border_xmax, border_ymax;
/** Escape factor, 0 if none. */
weight_t escape_factor;
- /** List of obstacles. */
- path_obstacle_t obstacles[PATH_OBSTACLES_NB];
/** Number of obstacles */
int obstacles_nb;
- /** List of navigation points coordonates */
- vect_t navpoints[PATH_NAVPOINTS_NB];
- /** List of navigation points weights */
- weight_t navweights[PATH_NAVPOINTS_NB];
/** Number of navigation points */
int navpoints_nb;
- /** List of nodes used for A*. */
- struct astar_node_t astar_nodes[PATH_NAVPOINTS_NB];
/** Which node to look at for next step. */
uint8_t next_node;
/** TRUE when a path has been found */