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/main.c | 28 ++++++++++++++++++++++++++-- digital/asserv/src/asserv/traj.c | 29 ++++++++++++++++++++++++----- digital/asserv/src/asserv/traj.h | 5 +++-- digital/asserv/src/asserv/twi_proto.c | 22 ++++++++++++++++++++-- 4 files changed, 73 insertions(+), 11 deletions(-) (limited to 'digital/asserv/src') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 47b2af2f..19fe2056 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -363,7 +363,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]), v8_to_v32 (args[4], args[5], args[6], args[7]), - args[8]); + 0, args[8]); + break; + case c ('r', 9): + /* Go to position, backward allowed. + * - d: x, f24.8. + * - d: y, f24.8. + * - b: sequence number. */ + if (args[8] == state_main.sequence) + break; + traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]), + v8_to_v32 (args[4], args[5], args[6], args[7]), + 1, args[8]); break; case c ('x', 3): /* Go to angle. @@ -385,7 +396,20 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]), v8_to_v32 (args[4], args[5], args[6], args[7]), v8_to_v32 (0, args[8], args[9], 0), - args[10]); + 0, args[10]); + break; + case c ('r', 11): + /* Go to position, then angle, backward allowed. + * - d: x, f24.8. + * - d: y, f24.8. + * - w: a, f0.16. + * - b: sequence number. */ + if (args[10] == state_main.sequence) + break; + traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]), + v8_to_v32 (args[4], args[5], args[6], args[7]), + v8_to_v32 (0, args[8], args[9], 0), + 1, args[10]); break; case c ('y', 3): /* Auxiliary go to position. 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; diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 5d09f082..8619f0bd 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -42,12 +42,13 @@ void traj_gtd_start (uint8_t seq); 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); void traj_goto_angle_start (uint32_t a, uint8_t seq); 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); #endif /* traj_h */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 85ad6d2c..76639064 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -149,7 +149,15 @@ twi_proto_callback (u8 *buf, u8 size) * - 3b: y position. */ traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0), v8_to_v32 (buf[5], buf[6], buf[7], 0), - 0); + 0, 0); + break; + case c ('r', 6): + /* Go to position, backward allowed. + * - 3b: x position. + * - 3b: y position. */ + traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0), + v8_to_v32 (buf[5], buf[6], buf[7], 0), + 1, 0); break; case c ('y', 2): /* Go to angle. @@ -164,7 +172,17 @@ twi_proto_callback (u8 *buf, u8 size) traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0), v8_to_v32 (buf[5], buf[6], buf[7], 0), v8_to_v32 (0, buf[8], buf[9], 0), - 0); + 0, 0); + break; + case c ('R', 8): + /* Go to position, then angle, backward allowed. + * - 3b: x position. + * - 3b: y position. + * - w: angle. */ + traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0), + v8_to_v32 (buf[5], buf[6], buf[7], 0), + v8_to_v32 (0, buf[8], buf[9], 0), + 1, 0); break; case c ('b', 3): /* Move the arm. -- cgit v1.2.3