From b0e46d53dacdcc37dd4379c32fe886e5776f76c7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 23 Apr 2008 21:02:33 +0200 Subject: * digital/asserv/src/asserv: - added backward goto commands. --- digital/asserv/src/asserv/traj.c | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) (limited to 'digital/asserv/src/asserv/traj.c') diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index e5281657..cdac0538 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -44,6 +44,12 @@ /* This file provides high level trajectories commands. */ +/** Angle after which a backward movement is considered. */ +#define TRAJ_GOTO_BACKWARD_THRESHOLD ((int32_t) (0.55 * M_PI * (1L << 24))) + +/** The famous number. */ +#define PI_F824 ((int32_t) (M_PI * (1L << 24))) + /** Traj mode enum. */ enum { @@ -76,11 +82,14 @@ static uint32_t traj_goto_x, traj_goto_y; /** Go to angle. */ static uint32_t traj_goto_a; +/** Allow backward movements. */ +static uint8_t traj_backward_ok; + /** Angle offset. Directly handled to speed layer. */ void traj_angle_offset_start (int32_t angle, uint8_t seq) { - int32_t a = fixed_mul_f824 (angle, 2 * M_PI * (1L << 24)); + int32_t a = fixed_mul_f824 (angle, 2 * PI_F824); uint32_t f = postrack_footing; int32_t arc = fixed_mul_f824 (f, a); speed_theta.use_pos = speed_alpha.use_pos = 1; @@ -187,11 +196,18 @@ traj_goto (void) int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s); /* Compute arc length. */ int32_t arad = atan2 (da, dt) * (1L << 24); + if (traj_backward_ok) + { + if (arad > TRAJ_GOTO_BACKWARD_THRESHOLD) + arad = - PI_F824 + arad; + else if (arad < - TRAJ_GOTO_BACKWARD_THRESHOLD) + arad = PI_F824 + arad; + } int32_t arc = fixed_mul_f824 (arad, postrack_footing); /* Compute consign. */ speed_alpha.pos_cons = pos_alpha.cur; speed_alpha.pos_cons += arc; - if (UTILS_ABS (arad) < 0.5 * M_PI * (1L << 24)) + if (UTILS_ABS (arad) < PI_F824 / 2) { speed_theta.pos_cons = pos_theta.cur; speed_theta.pos_cons += dt >> 8; @@ -205,11 +221,12 @@ traj_goto (void) /** Start go to position mode (x, y: f24.8). */ void -traj_goto_start (uint32_t x, uint32_t y, uint8_t seq) +traj_goto_start (uint32_t x, uint32_t y, uint8_t backward_ok, uint8_t seq) { traj_mode = TRAJ_GOTO; traj_goto_x = x; traj_goto_y = y; + traj_backward_ok = backward_ok; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_alpha.pos_cons = pos_alpha.cons; @@ -232,7 +249,7 @@ traj_goto_angle (void) { /* Compute arc length. */ int32_t arad = fixed_mul_f824 (((int32_t) da) << 8, - 2 * M_PI * (1L << 24)); + 2 * PI_F824); int32_t arc = fixed_mul_f824 (arad, postrack_footing); /* Compute consign. */ speed_alpha.pos_cons = pos_alpha.cur; @@ -273,12 +290,14 @@ traj_goto_xya (void) /** Start go to position, then angle mode (x, y: f24.8, a: f8.24). */ void -traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t seq) +traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward_ok, + uint8_t seq) { traj_mode = TRAJ_GOTO_XYA; traj_goto_x = x; traj_goto_y = y; traj_goto_a = a; + traj_backward_ok = backward_ok; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_alpha.pos_cons = pos_alpha.cons; -- cgit v1.2.3