From 224fa599aa45b137b3c2236dc60adfbe059efa25 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Tue, 16 Apr 2013 20:13:56 +0200 Subject: digital/io-hub/src/common-cc: fix the weight of the second layer of navigation points --- digital/io-hub/src/common-cc/path.cc | 30 ++++++++++++++++-------------- digital/io-hub/src/common-cc/path.hh | 8 ++++++-- 2 files changed, 22 insertions(+), 16 deletions(-) (limited to 'digital/io-hub/src/common-cc') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 99b9b3f1..c8ae6d77 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -86,12 +86,12 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) { uint32_t rot_a, rot_b, nr; uint32_t x, y, nx; - int npt, weight; + int npt, layer; DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, nodes); /* Enlarge the obstacle radius by the robot size and clearance area width */ - r += BOT_SIZE_SIDE + Obstacles::clearance_mm; + r += BOT_SIZE_RADIUS/*BOT_SIZE_SIDE*/ + Obstacles::clearance_mm; /* Store obstacle */ //assert(obstacles_nb < PATH_OBSTACLES_NB); @@ -111,8 +111,9 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) x = fixed_div_f824(r, fixed_cos_f824(nr)) + 1 /* margin */; y = 0; - /* Add 2 sets of navigation points with different weights */ - for(weight=PATH_NAVPOINTS_LAYERS; weight>0; weight--) + /* Add a number of sets of navigation points with different weights */ + //for(weight=PATH_NAVPOINTS_LAYERS; weight>0; weight--) + for(layer=PATH_NAVPOINTS_LAYERS-1; layer>=0; layer--) { /* Compute obstacle points positions around a circle */ for (npt=0; npt= border_xmin - && navpoints[navpoints_nb].y >= border_ymin - && navpoints[navpoints_nb].x <= border_xmax - && navpoints[navpoints_nb].y <= border_ymax) + && navpoints[navpoints_nb].y >= border_ymin + && navpoints[navpoints_nb].x <= border_xmax + && navpoints[navpoints_nb].y <= border_ymax) { /* Accept point */ + navweights[navpoints_nb] = (1<>= PATH_WEIGHT_PRECISION; + DPRINTF("- Node %u (%u;%u) w=%u (%u) ", i, navpoints[i].x, navpoints[i].y, weight, navweights[i]); /* Check every obstacle */ for(int j=0; j> Path found: "); while(node!=PATH_NAVPOINT_DST_IDX) { - DPRINTF("%u, ", node); + DPRINTF("%u (%u), ", node, navweights[node]); path[path_nb++] = navpoints[node]; node = astar_nodes[node].prev; } @@ -264,9 +266,9 @@ void Path::endpoints(const vect_t &src, const vect_t &dst) 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; - navweights[PATH_NAVPOINT_SRC_IDX] = 1; + navweights[PATH_NAVPOINT_SRC_IDX] = (1<