From 33a67790aa0df5a0461dc3c5425d5c2a14d282b8 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Thu, 25 Apr 2013 20:27:40 +0200 Subject: digital/io-hub/src/common-cc: enlarge the obstacles clearance radius --- digital/io-hub/src/common-cc/path.cc | 21 +++++++++++++-------- digital/io-hub/src/common-cc/path.hh | 14 ++++++++------ 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 8d709c9a..fe62496a 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -49,6 +49,9 @@ extern "C" { /** Check for vectors equality */ #define PATH_VECT_EQUAL(v1, v2) ((v1)->x==(v2)->x && (v1)->y==(v2)->y) +/** Check if point is inside a circle */ +#define PATH_IN_CIRCLE(pOINT, cENTER, rADIUS) (distance_point_point((pOINT), (cENTER)) <= (rADIUS)) + /** Static nodes index for the endpoints */ enum { PATH_NAVPOINT_SRC_IDX = 0, @@ -83,20 +86,20 @@ void Path::reset() #ifdef playground_2013_hh /* Declare the cake as an obstacle */ - add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_NAVPOINTS_NB); + add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_NAVPOINTS_NB, PATH_CAKE_NAVPOINTS_LAYERS, 0 /* no extra clearance radius */); #endif } -void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) +void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance) { uint32_t rot_a, rot_b, nr; uint32_t x, y, nx; int npt, layer; - DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, nodes); + DPRINTF("Add obstacle c=(%u;%u) r=%u nodes=%u layers=%u\n",c.x, c.y, r, nodes, nlayers); /* Enlarge the obstacle radius by the robot size and clearance area width */ - r += BOT_SIZE_RADIUS/*BOT_SIZE_SIDE*/ + Obstacles::clearance_mm; + r += BOT_SIZE_RADIUS /*BOT_SIZE_SIDE*/ + clearance; /* Store obstacle */ //assert(obstacles_nb < PATH_OBSTACLES_NB); @@ -117,8 +120,8 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) y = 0; /* 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--) + //for(weight=PATH_OBSTACLES_NAVPOINTS_LAYERS; weight>0; weight--) + for(layer=nlayers-1; layer>=0; layer--) { /* Compute obstacle points positions around a circle */ for (npt=0; npt