summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/common-cc/path.cc20
1 files changed, 16 insertions, 4 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index 32aed5f0..bb947212 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -97,6 +97,7 @@ void Path::reset()
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 rot_c, rot_d;
uint32_t x, y, nx;
int npt, layer;
@@ -111,16 +112,20 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int
obstacles[obstacles_nb].r = r;
obstacles_nb++;
- /* Complex number of modulus 1 and rotation angle */
+ /* 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) */
- nr = PATH_ANGLE_824(nodes*2);
- x = fixed_div_f824(r, fixed_cos_f824(nr)) + 2 /* margin for the unprecise fixed point computation */;
+ x = fixed_div_f824(r, rot_c) + 2 /* margin for the unprecise fixed point computation */;
y = 0;
/* Add a number of sets of navigation points with different weights */
@@ -152,8 +157,15 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int
x = nx;
}
+ /* Prepare the next layer */
/* Enlarge the circle */
- x += BOT_SIZE_RADIUS;
+ x += BOT_SIZE_RADIUS; y = 0;
+
+ /* 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;
}
#ifdef HOST