summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/apbirthday/path_2013.hh4
-rw-r--r--digital/io-hub/src/common-cc/path.cc45
-rw-r--r--digital/io-hub/src/common-cc/path.hh24
3 files changed, 36 insertions, 37 deletions
diff --git a/digital/io-hub/src/apbirthday/path_2013.hh b/digital/io-hub/src/apbirthday/path_2013.hh
index 17392aad..4018727d 100644
--- a/digital/io-hub/src/apbirthday/path_2013.hh
+++ b/digital/io-hub/src/apbirthday/path_2013.hh
@@ -40,14 +40,14 @@ class Path_2013 : public Path
private:
/** Number of additional obstacles on the playground */
- static const int PATH_2013_OBSTACLES_NB = PATH_OBSTACLES_NB + 1/*cake*/;
+ static const int PATH_2013_OBSTACLES_NB = CC_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;
+ CC_NAVPOINTS_NB + PATH_2013_CAKE_NAVPOINTS_LAYERS * PATH_2013_CAKE_NAVPOINTS_NB;
};
#endif // path_2013_hh
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index 1340df64..77ab1869 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -87,9 +87,9 @@ void Path::reset()
vect_t nul = {0,0};
host_debug("Path reset\n");
obstacles_nb = 0;
- navpoints_nb = PATH_RESERVED_NAVPOINTS_NB;
- navpoints[PATH_NAVPOINT_SRC_IDX] = nul;
- navpoints[PATH_NAVPOINT_DST_IDX] = nul;
+ navpoints_nb = CC_RESERVED_NAVPOINTS_NB;
+ navpoints[CC_NAVPOINT_SRC_IDX] = nul;
+ navpoints[CC_NAVPOINT_DST_IDX] = nul;
next_node = 0;
escape_factor = 0;
}
@@ -111,7 +111,6 @@ void Path::add_obstacle( const vect_t &c,
r += BOT_SIZE_RADIUS;
/* Store obstacle */
- //assert(obstacles_nb < PATH_OBSTACLES_NB);
obstacles[obstacles_nb].c = c;
obstacles[obstacles_nb].r = r;
obstacles[obstacles_nb].target_allowed = target;
@@ -150,7 +149,7 @@ void Path::add_obstacle( const vect_t &c,
&& navpoints[navpoints_nb].y <= border_ymax)
{
/* Accept point */
- navweights[navpoints_nb] = (layer * PATH_WEIGHT_STEP);
+ navweights[navpoints_nb] = (layer * CC_NAVWEIGHT_STEP);
host_debug("Add point %u (%u;%u) w=%u\n",
navpoints_nb, navpoints[navpoints_nb].x, navpoints[navpoints_nb].y, navweights[navpoints_nb]);
navpoints_nb++;
@@ -172,7 +171,7 @@ void Path::add_obstacle( const vect_t &c,
#ifdef HOST
/* Plot obstacle points */
- robot->hardware.simu_report.pos( &navpoints[PATH_RESERVED_NAVPOINTS_NB], navpoints_nb-PATH_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID);
+ robot->hardware.simu_report.pos( &navpoints[CC_RESERVED_NAVPOINTS_NB], navpoints_nb-CC_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID);
#endif
}
@@ -191,7 +190,7 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
/* 1st: compute the distance to go */
weight_t weight = (weight_t)distance_point_point(&navpoints[cur_point], &navpoints[next_point]);
/* 2nd: Add the target navpoint extra weigth */
- weight += (weight * navweights[cur_point]) >> PATH_WEIGHT_PRECISION;
+ weight += (weight * navweights[cur_point]) >> CC_NAVWEIGHT_PRECISION;
host_debug("- Node %u (%u;%u) w=%u (%u) ", next_point, navpoints[next_point].x, navpoints[next_point].y, weight, navweights[next_point]);
/* Check every obstacle */
@@ -206,7 +205,7 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
/* target the center of an obstacle and stop */
/* in front of it (ex, the cake in apbirthday 2013) */
if (obstacles[j].target_allowed &&
- cur_point==PATH_NAVPOINT_DST_IDX &&
+ cur_point==CC_NAVPOINT_DST_IDX &&
PATH_VECT_EQUAL(&navpoints[cur_point], &obstacles[j].c))
{
/* Skip this obstacle */
@@ -217,7 +216,7 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors)
/* Collision while trying to escape the source point */
/* if and only if the source point is in this obstacle */
/* and we are going away from the obstacle center */
- else if (escape_factor!=0 && next_point==PATH_NAVPOINT_SRC_IDX &&
+ else if (escape_factor!=0 && next_point==CC_NAVPOINT_SRC_IDX &&
PATH_IN_CIRCLE(&navpoints[next_point], &obstacles[j].c, obstacles[j].r) &&
pos_dot_product(&navpoints[next_point], &obstacles[j].c, &navpoints[next_point], &navpoints[cur_point])<=0)
{
@@ -269,20 +268,20 @@ void Path::compute(weight_t escape)
escape_factor = escape;
/* Call the A* algorithm */
- path_found = (bool)astar(astar_nodes, navpoints_nb, PATH_NAVPOINT_DST_IDX, PATH_NAVPOINT_SRC_IDX);
+ path_found = (bool)astar(astar_nodes, navpoints_nb, CC_NAVPOINT_DST_IDX, CC_NAVPOINT_SRC_IDX);
if (path_found)
{
/* Store next node to go to */
- next_node = astar_nodes[PATH_NAVPOINT_SRC_IDX].prev;
+ next_node = astar_nodes[CC_NAVPOINT_SRC_IDX].prev;
#ifdef PATH_DEBUG
/* Log and display the path found */
vect_t path[navpoints_nb];
- int node = PATH_NAVPOINT_SRC_IDX;
+ int node = CC_NAVPOINT_SRC_IDX;
int path_nb = 0;
host_debug(">> Path found: ");
- while(node!=PATH_NAVPOINT_DST_IDX)
+ while(node!=CC_NAVPOINT_DST_IDX)
{
host_debug("%u (%u), ", node, navweights[node]);
path[path_nb++] = navpoints[node];
@@ -300,10 +299,10 @@ void Path::compute(weight_t escape)
void Path::obstacle(const int index, const vect_t &c, const uint16_t r, const int f, const bool target)
{
add_obstacle( c,
- (r + Obstacles::clearance_mm + PATH_OBSTACLES_CLEARANCE),
- PATH_OBSTACLES_NAVPOINTS_NB,
- PATH_OBSTACLES_NAVPOINTS_LAYERS,
- PATH_NAVPOINTS_CLEARANCE,
+ (r + Obstacles::clearance_mm + CC_OBSTACLES_CLEARANCE),
+ CC_OBSTACLE_NAVPOINTS_NB,
+ CC_OBSTACLE_NAVPOINTS_LAYERS,
+ CC_NAVPOINTS_CLEARANCE,
target);
}
@@ -312,12 +311,12 @@ void Path::endpoints(const vect_t &src, const vect_t &dst)
/* Store endpoints location */
host_debug("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;
+ navpoints[CC_NAVPOINT_SRC_IDX] = src;
+ navpoints[CC_NAVPOINT_DST_IDX] = dst;
/* Init endpoints weights */
- navweights[PATH_NAVPOINT_SRC_IDX] = 0;
- navweights[PATH_NAVPOINT_DST_IDX] = 0;
+ navweights[CC_NAVPOINT_SRC_IDX] = 0;
+ navweights[CC_NAVPOINT_DST_IDX] = 0;
}
bool Path::get_next(vect_t &p)
@@ -351,7 +350,7 @@ 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, navpoints_nb, get_point_index(src), PATH_NAVPOINT_DST_IDX);
+ astar_dijkstra_prepare(astar_nodes, navpoints_nb, get_point_index(src), CC_NAVPOINT_DST_IDX);
}
weight_t Path::get_score(const vect_t &dst)
@@ -375,7 +374,7 @@ extern "C" uint16_t
AC_ASTAR_HEURISTIC_CALLBACK (uint8_t node)
{
vect_t point_a = robot->path.get_point_vect(node);
- vect_t point_b = robot->path.get_point_vect(PATH_NAVPOINT_SRC_IDX);
+ vect_t point_b = robot->path.get_point_vect(CC_NAVPOINT_SRC_IDX);
int16_t dist = distance_point_point(&point_a, &point_b);
host_debug("Heuristic node=%u dist=%u\n", node, dist);
return dist;
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index d673d0cf..7806e537 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -32,9 +32,9 @@ extern "C" {
/** Static nodes index for the endpoints */
enum {
- PATH_NAVPOINT_SRC_IDX = 0,
- PATH_NAVPOINT_DST_IDX,
- PATH_RESERVED_NAVPOINTS_NB
+ CC_NAVPOINT_SRC_IDX = 0,
+ CC_NAVPOINT_DST_IDX,
+ CC_RESERVED_NAVPOINTS_NB
};
/** Obstacle */
@@ -85,14 +85,14 @@ class Path
/** 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 mobile obstacles. */
- static const int PATH_OBSTACLES_NB = 4;
+ static const int CC_OBSTACLES_NB = 4;
/** Number of points per standard obstacle. */
- static const int PATH_OBSTACLES_NAVPOINTS_NB = 10;
+ static const int CC_OBSTACLE_NAVPOINTS_NB = 10;
/** Number of navigation points layers for each obstacle. */
- static const int PATH_OBSTACLES_NAVPOINTS_LAYERS = 2;
+ static const int CC_OBSTACLE_NAVPOINTS_LAYERS = 2;
/** Number of navigation points. */
- static const int PATH_NAVPOINTS_NB =
- (PATH_RESERVED_NAVPOINTS_NB + PATH_OBSTACLES_NAVPOINTS_LAYERS * PATH_OBSTACLES_NB * PATH_OBSTACLES_NAVPOINTS_NB);
+ static const int CC_NAVPOINTS_NB =
+ (CC_RESERVED_NAVPOINTS_NB + CC_OBSTACLE_NAVPOINTS_LAYERS * CC_OBSTACLES_NB * CC_OBSTACLE_NAVPOINTS_NB);
/** Borders, any point outside borders is eliminated. */
const uint16_t border_xmin, border_ymin, border_xmax, border_ymax;
/** List of obstacles. */
@@ -107,15 +107,15 @@ class Path
private:
/** Navigation points weight precision (2^-n).
* Pay attention to overflow on weight_t variables */
- static const int PATH_WEIGHT_PRECISION = 4;
+ static const int CC_NAVWEIGHT_PRECISION = 4;
/** Navigation points weight step * (2^-n). */
- static const int PATH_WEIGHT_STEP = 8;
+ static const int CC_NAVWEIGHT_STEP = 8;
/** Extra clearance area added to the radius of the mobile obstacles
* to counter the imprecision of the sonic sensors when the robot brakes */
- static const uint16_t PATH_OBSTACLES_CLEARANCE = 60;
+ static const uint16_t CC_OBSTACLES_CLEARANCE = 60;
/** 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;
+ static const uint16_t CC_NAVPOINTS_CLEARANCE = 40;
/** Escape factor, 0 if none. */
weight_t escape_factor;
/** Number of obstacles */