From 0f5fb480d954339158033be901a19f15c3815847 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 20 Apr 2008 19:18:20 +0200 Subject: * digital/asserv/src/asserv: - added goto_xya command. - reduced the goto_angle proto parameter size. * digital/io/doc: - added goto_xya command. * digital/asserv/tools: - added goto_xya command. --- digital/asserv/src/asserv/main.c | 23 ++++++++++++++++----- digital/asserv/src/asserv/traj.c | 38 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 3 +++ digital/asserv/src/asserv/twi_proto.c | 14 +++++++++++-- digital/asserv/tools/asserv.py | 10 ++++++++- digital/io/doc/proto_asserv.txt | 4 ++++ 6 files changed, 84 insertions(+), 8 deletions(-) diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 0a746149..f0813fba 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -361,14 +361,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v32 (args[4], args[5], args[6], args[7]), args[8]); break; - case c ('x', 5): + case c ('x', 3): /* Go to angle. - * - d: a, f8.24. + * - d: a, f0.16. * - b: sequence number. */ - if (args[4] == state_main.sequence) + if (args[2] == state_main.sequence) + break; + traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0), + args[2]); + break; + case c ('x', 11): + /* Go to position, then angle. + * - d: x, f24.8. + * - d: y, f24.8. + * - w: a, f0.16. + * - b: sequence number. */ + if (args[10] == state_main.sequence) break; - traj_goto_angle_start (v8_to_v32 (args[0], args[1], args[2], args[3]), - args[4]); + 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]); break; case c ('a', 2): /* Set both acknoledge. diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 243df1c8..e5281657 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -55,6 +55,8 @@ enum TRAJ_GOTO, /* Go to angle. */ TRAJ_GOTO_ANGLE, + /* Go to position, then angle. */ + TRAJ_GOTO_XYA, /* Everything done. */ TRAJ_DONE, }; @@ -250,6 +252,39 @@ traj_goto_angle_start (uint32_t a, uint8_t seq) state_start (&state_main, MODE_TRAJ, seq); } +/** Go to position, then angle mode. */ +static void +traj_goto_xya (void) +{ + int32_t dx = traj_goto_x - postrack_x; + int32_t dy = traj_goto_y - postrack_y; + if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8 + && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8) + { + /* Near enough, now do a go to angle. */ + traj_mode = TRAJ_GOTO_ANGLE; + traj_goto_angle (); + } + else + { + traj_goto (); + } +} + +/** 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_mode = TRAJ_GOTO_XYA; + traj_goto_x = x; + traj_goto_y = y; + traj_goto_a = a; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, MODE_TRAJ, seq); +} + /* Compute new speed according the defined trajectory. */ void traj_update (void) @@ -268,6 +303,9 @@ traj_update (void) case TRAJ_GOTO_ANGLE: traj_goto_angle (); break; + case TRAJ_GOTO_XYA: + traj_goto_xya (); + break; case TRAJ_DONE: break; } diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index fe9830c9..5d09f082 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -47,4 +47,7 @@ traj_goto_start (uint32_t x, uint32_t y, 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); + #endif /* traj_h */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 171a6895..c7564766 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -139,7 +139,7 @@ twi_proto_callback (u8 *buf, u8 size) /* Go to the dispenser. */ traj_gtd_start (0); break; - case c ('x', 0): + case c ('x', 6): /* Go to position. * - 3b: x position. * - 3b: y position. */ @@ -147,11 +147,21 @@ twi_proto_callback (u8 *buf, u8 size) v8_to_v32 (buf[5], buf[6], buf[7], 0), 0); break; - case c ('y', 0): + case c ('y', 2): /* Go to angle. * - w: angle. */ traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0); break; + case c ('X', 8): + /* Go to position, then angle. + * - 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), + 0); + break; case c ('b', 3): /* Move the arm. * - w: new position. diff --git a/digital/asserv/tools/asserv.py b/digital/asserv/tools/asserv.py index 2687099f..6260f0ef 100644 --- a/digital/asserv/tools/asserv.py +++ b/digital/asserv/tools/asserv.py @@ -145,7 +145,15 @@ class Asserv: def goto_angle (self, a): """Go to angle.""" self.seq += 1 - self.proto.send ('x', 'LB', a * (1 << 24) / 360, self.seq) + self.proto.send ('x', 'HB', a * (1 << 16) / 360, self.seq) + self.wait (self.finished) + + def goto_xya (self, x, y, a): + """Go to position, then angle.""" + self.seq += 1 + self.proto.send ('x', 'LLHB', 256 * x / self.param['scale'], + 256 * y / self.param['scale'], + a * (1 << 16) / 360, self.seq) self.wait (self.finished) def send_param (self): diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt index 843e8dd0..43c85948 100644 --- a/digital/io/doc/proto_asserv.txt +++ b/digital/io/doc/proto_asserv.txt @@ -79,6 +79,10 @@ This table describe the list of supported commands by the *asserv* card: +---------+-------+-----------------+------------------------------------------+ | '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) | | ++---------+-------+-----------------+------------------------------------------+ | '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