From f5905f31bdfb051aefe0cf04e0caef976027cc2b Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Fri, 3 May 2013 19:03:39 +0200 Subject: digital/io-hub/src: rename Path and Path_2013 constants for clearness --- digital/io-hub/src/apbirthday/path_2013.hh | 4 +-- digital/io-hub/src/common-cc/path.cc | 45 +++++++++++++++--------------- digital/io-hub/src/common-cc/path.hh | 24 ++++++++-------- 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 */ -- cgit v1.2.3