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 ++++++++++++++++++++-- digital/asserv/tools/asserv.py | 10 ++++++---- digital/io/doc/proto_asserv.txt | 7 +++++++ 6 files changed, 86 insertions(+), 15 deletions(-) (limited to 'digital') 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. diff --git a/digital/asserv/tools/asserv.py b/digital/asserv/tools/asserv.py index 6260f0ef..e1bfa5e9 100644 --- a/digital/asserv/tools/asserv.py +++ b/digital/asserv/tools/asserv.py @@ -135,10 +135,11 @@ class Asserv: self.proto.send ('s', 'LB', offset, self.seq) self.wait (self.finished) - def goto (self, x, y): + def goto (self, x, y, backward_ok = False): """Go to position.""" self.seq += 1 - self.proto.send ('x', 'LLB', 256 * x / self.param['scale'], + self.proto.send (backward_ok and 'r' or 'x', 'LLB', + 256 * x / self.param['scale'], 256 * y / self.param['scale'], self.seq) self.wait (self.finished) @@ -148,10 +149,11 @@ class Asserv: self.proto.send ('x', 'HB', a * (1 << 16) / 360, self.seq) self.wait (self.finished) - def goto_xya (self, x, y, a): + def goto_xya (self, x, y, a, backward_ok = False): """Go to position, then angle.""" self.seq += 1 - self.proto.send ('x', 'LLHB', 256 * x / self.param['scale'], + self.proto.send (backward_ok and 'r' or 'x', 'LLHB', + 256 * x / self.param['scale'], 256 * y / self.param['scale'], a * (1 << 16) / 360, self.seq) self.wait (self.finished) diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt index 8573c81e..2fffb618 100644 --- a/digital/io/doc/proto_asserv.txt +++ b/digital/io/doc/proto_asserv.txt @@ -77,12 +77,19 @@ This table describe the list of supported commands by the *asserv* card: | 'x' | Move | - x (3b) | Go to an absolute position | | | | - y (3b) | | +---------+-------+-----------------+------------------------------------------+ +| 'r' | Move | - x (3b) | Go to an absolute position, backward | +| | | - y (3b) | movement allowed | ++---------+-------+-----------------+------------------------------------------+ | 'y' | Move | - a (2b) | Go to an absolute angle | +---------+-------+-----------------+------------------------------------------+ | 'X' | Move | - x (3b) | Go to an absolute position, then an | | | | - y (3b) | absolute angle | | | | - a (2b) | | +---------+-------+-----------------+------------------------------------------+ +| 'R' | Move | - x (3b) | Go to an absolute position, then an | +| | | - y (3b) | absolute angle, backward movement | +| | | - a (2b) | allowed | ++---------+-------+-----------------+------------------------------------------+ | 'b' | Arm | - position (2b) | Move the arm to a desired position | | | | - speed (1b) | at a specific speed | | | | | - position: 5000 steps by round | -- cgit v1.2.3