summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-05-03 16:06:38 +0200
committerNicolas Schodet2013-05-03 23:07:15 +0200
commit496c1b542fcc390aa1cd142a5b86aa82ae1b3c25 (patch)
treebfe4275c5374cda71f65156cc588075d745ca949
parent3bea178fdc550e0e8a9103d9cb3eb62016077282 (diff)
digital/io-hub/src/common-cc/path: reduce the number of navigation points around the obstacles
On the second navigation points layer, this number is divided by 2. More points seems unnecessary.
-rw-r--r--digital/io-hub/src/common-cc/path.cc36
-rw-r--r--digital/io-hub/src/common-cc/path.hh4
2 files changed, 16 insertions, 24 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index 463daca5..c34dd6a4 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -102,14 +102,13 @@ void Path::reset()
void Path::add_obstacle( const vect_t &c,
uint16_t r,
- const int nodes,
+ int nodes,
const int nlayers,
const uint16_t clearance,
const bool target)
{
uint32_t rot_a, rot_b, nr;
- uint32_t rot_c, rot_d;
- uint32_t x, y, nx;
+ int32_t x, y, nx;
int npt, layer;
host_debug("Add obstacle c=(%u;%u) r=%u nodes=%u layers=%u\n",c.x, c.y, r, nodes, nlayers);
@@ -124,25 +123,21 @@ void Path::add_obstacle( const vect_t &c,
obstacles[obstacles_nb].target_allowed = target;
obstacles_nb++;
- /* Complex number A = cos(angle) + i sin(angle) */
- nr = PATH_ANGLE_824(nodes);
- rot_a = fixed_cos_f824(nr);
- rot_b = fixed_sin_f824(nr);
-
- /* Complex number B = cos(angle/2) + i sin(angle/2) */
- nr = PATH_ANGLE_824(nodes*2);
- rot_c = fixed_cos_f824(nr);
- rot_d = fixed_sin_f824(nr);
-
/* Extend the points radius to allow the robot to go */
/* from one to another without collision with the */
/* obstacle circle. New radius is r / cos(angle/2) */
- x = fixed_div_f824(r, rot_c) + 3 /* margin for the unprecise fixed point computation */;
- y = 0;
+ nr = PATH_ANGLE_824(nodes*2);
+ y = fixed_div_f824(r, fixed_cos_f824(nr)) + 3 /* margin for the unprecise fixed point computation */;
+ x = 0;
/* Add a number of sets of navigation points with different weights */
for(layer=nlayers-1; layer>=0; layer--)
{
+ /* Complex number A = cos(angle) + i sin(angle) */
+ nr = PATH_ANGLE_824(nodes);
+ rot_a = fixed_cos_f824(nr);
+ rot_b = fixed_sin_f824(nr);
+
/* Compute obstacle points positions around a circle */
for (npt=0; npt<nodes; npt++)
{
@@ -170,14 +165,11 @@ void Path::add_obstacle( const vect_t &c,
}
/* Prepare the next layer */
- /* Enlarge the circle */
- x += BOT_SIZE_RADIUS; y = 0;
+ /* Twice less navpoints */
+ nodes >>= 1;
- /* Rotate the next point by (angle/2) */
- /* This interleaves the new navpoints with the previous ones */
- nx = fixed_mul_f824(x, rot_c) - fixed_mul_f824(y, rot_d);
- y = fixed_mul_f824(y, rot_c) + fixed_mul_f824(x, rot_d);
- x = nx;
+ /* Enlarge the circle */
+ y += BOT_SIZE_RADIUS; x = 0;
}
#ifdef HOST
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index 74a8a382..fc743291 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -84,12 +84,12 @@ class Path
private:
/** Add an obstacle on the field */
- void add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance, const bool target);
+ 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 obstacles. */
static const int PATH_OBSTACLES_NB = (4+1/*cake*/);
/** Number of points per standard obstacle. */
- static const int PATH_OBSTACLES_NAVPOINTS_NB = 12;
+ static const int PATH_OBSTACLES_NAVPOINTS_NB = 10;
#ifdef playground_2013_hh
/** Number of points for the cake (plus 1) */
static const int PATH_CAKE_NAVPOINTS_NB = 6;