From 4e67c3e2e763eab19d253fad78331a0b4ac6625a Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Thu, 18 Apr 2013 02:59:36 +0200 Subject: digital/io-hub/src/common-cc: allow robot to target an obstacle (the cake) --- digital/io-hub/src/common-cc/path.cc | 78 ++++++++++++++++++++++++------------ digital/io-hub/src/common-cc/path.hh | 15 +++++-- 2 files changed, 64 insertions(+), 29 deletions(-) diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 6febd082..8d709c9a 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -28,7 +28,6 @@ #include "bot.hh" #include "robot.hh" #include "playground.hh" -#include "playground_2013.hh" extern "C" { #include "modules/math/fixed/fixed.h" @@ -47,6 +46,10 @@ extern "C" { /** Angle between obstacles points. */ #define PATH_ANGLE_824(pOINTS_NB) ((1L << 24) / (pOINTS_NB)) +/** Check for vectors equality */ +#define PATH_VECT_EQUAL(v1, v2) ((v1)->x==(v2)->x && (v1)->y==(v2)->y) + +/** Static nodes index for the endpoints */ enum { PATH_NAVPOINT_SRC_IDX = 0, PATH_NAVPOINT_DST_IDX @@ -78,8 +81,10 @@ void Path::reset() 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); +#endif } void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) @@ -148,11 +153,6 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) #ifdef HOST /* Plot obstacle points */ robot->hardware.simu_report.pos( &navpoints[PATH_RESERVED_NAVPOINTS_NB], navpoints_nb-PATH_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID); -#if 0 - /* Draw the last obstacle */ - navpoints[navpoints_nb] = navpoints[navpoints_nb - num]; - robot->hardware.simu_report.path( &navpoints[navpoints_nb - num], num + 1); -#endif #endif } @@ -175,27 +175,37 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) for(int j=0; j validated w=%u\n", weight); @@ -261,14 +271,30 @@ void Path::obstacle(int index, const vect_t &c, uint16_t r, int f) add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB); } -void Path::endpoints(const vect_t &src, const vect_t &dst) +void Path::endpoints(const vect_t &src, const vect_t &dst, const bool force_move) { + /* Store endpoints location */ DPRINTF("Set path endpoints src=(%u;%u) dst=(%u;%u)\n", src.x, src.y, dst.x, dst.y); navpoints[PATH_NAVPOINT_SRC_IDX] = src; navpoints[PATH_NAVPOINT_DST_IDX] = dst; + + /* Init endpoints weights */ navweights[PATH_NAVPOINT_SRC_IDX] = (1<force_move = force_move; + +#ifdef playground_2013_hh + /* Temporary code for the cake */ + if (PATH_VECT_EQUAL(&dst, &pg_cake_pos)) + { + this->force_move = true; + } +#endif } bool Path::get_next(vect_t &p) diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh index 4a3cb072..8003f32b 100644 --- a/digital/io-hub/src/common-cc/path.hh +++ b/digital/io-hub/src/common-cc/path.hh @@ -25,6 +25,7 @@ // // }}} #include "defs.hh" +#include "playground_2013.hh" extern "C" { #include "modules/path/astar/astar.h" @@ -52,7 +53,7 @@ class Path /** Set a moving obstacle position, radius and factor*/ void obstacle(int index, const vect_t &c, uint16_t r, int f = 0); /** Set path source and destination */ - void endpoints(const vect_t &src, const vect_t &dst); + void endpoints(const vect_t &src, const vect_t &dst, const bool force_move=false); /** Compute path with the given escape factor */ void compute(weight_t escape = 0); /** Return a point vector by index */ @@ -78,16 +79,20 @@ class Path static const int PATH_OBSTACLES_NB = (4+1/*cake*/); /** Number of points per standard obstacle. */ static const int PATH_OBSTACLES_NAVPOINTS_NB = 8; +#ifdef playground_2013_hh /** Number of points for the cake */ static const int PATH_CAKE_NAVPOINTS_NB = 14; +#endif /** Number of reserved points for the 2 endpoints */ static const int PATH_RESERVED_NAVPOINTS_NB = 2; /** Number of navigation points layers for each obstacle. */ static const int PATH_NAVPOINTS_LAYERS = 2; /** Number of navigation points. */ static const int PATH_NAVPOINTS_NB = (PATH_RESERVED_NAVPOINTS_NB + - PATH_NAVPOINTS_LAYERS * (PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB) + - PATH_NAVPOINTS_LAYERS * (PATH_CAKE_NAVPOINTS_NB - PATH_OBSTACLES_NAVPOINTS_NB)); +#ifdef playground_2013_hh + PATH_NAVPOINTS_LAYERS * (PATH_CAKE_NAVPOINTS_NB - PATH_OBSTACLES_NAVPOINTS_NB) + +#endif + PATH_NAVPOINTS_LAYERS * (PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB)); /** Navigation points weight precision (2^-n). */ static const int PATH_WEIGHT_PRECISION = 4; /** Navigation points weight step (2^-n). */ @@ -113,6 +118,10 @@ class Path int next_node; /** TRUE when a path has been found */ bool path_found; + /** TRUE to allow last movement to enter an obstacle + * This may be used to target the center of an obstacle + * and stop closed to it */ + bool force_move; }; #endif // path_hh -- cgit v1.2.3