From 99f271d6ad074e2d592b48b50a325ee241cdbe34 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 9 Apr 2008 14:28:58 +0200 Subject: * digital/asserv/src/asserv: - first goto xy implementation. --- digital/asserv/src/asserv/eeprom.avr.c | 3 ++ digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 15 ++++++++ digital/asserv/src/asserv/traj.c | 64 ++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 4 +++ digital/asserv/src/asserv/twi_proto.c | 8 +++++ 6 files changed, 95 insertions(+), 1 deletion(-) diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 96611d3b..9f4920a4 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -31,6 +31,7 @@ #include "pos.h" #include "speed.h" #include "postrack.h" +#include "traj.h" #define EEPROM_START 0 @@ -71,6 +72,7 @@ eeprom_read_params (void) pos_e_sat = eeprom_read_word (p16++); pos_int_sat = eeprom_read_word (p16++); pos_blocked = eeprom_read_word (p16++); + traj_eps = eeprom_read_word (p16++); } /* Write parameters to eeprom. */ @@ -104,6 +106,7 @@ eeprom_write_params (void) eeprom_write_word (p16++, pos_e_sat); eeprom_write_word (p16++, pos_int_sat); eeprom_write_word (p16++, pos_blocked); + eeprom_write_word (p16++, traj_eps); } /* Clear eeprom parameters. */ diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index 95bb4e64..2f3d5c74 100644 --- a/digital/asserv/src/asserv/eeprom.h +++ b/digital/asserv/src/asserv/eeprom.h @@ -26,7 +26,7 @@ * }}} */ /** Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x46 +#define EEPROM_KEY 0x47 void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index c4e601df..60aef03d 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -348,6 +348,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_gtd_start (args[0]); break; + case c ('x', 9): + /* Go to position. + * - 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]), + args[8]); + break; case c ('a', 2): /* Set both acknoledge. * - b: main ack sequence number. @@ -495,6 +506,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('b', 3): pos_blocked = v8_to_v16 (args[1], args[2]); break; + case c ('e', 3): + traj_eps = v8_to_v16 (args[1], args[2]); + break; case c ('w', 2): /* Set PWM direction. * - b: bits: 0000[aux0][right][left]. */ @@ -526,6 +540,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) proto_send1w ('E', pos_e_sat); proto_send1w ('I', pos_int_sat); proto_send1w ('b', pos_blocked); + proto_send1w ('e', traj_eps); proto_send1b ('w', pwm_reverse); break; default: diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index ce8a7585..19995818 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -27,8 +27,11 @@ #include "modules/math/fixed/fixed.h" #include "modules/math/math.h" +#include "modules/utils/utils.h" #include "io.h" +#include + #include "state.h" #include "pos.h" @@ -48,6 +51,8 @@ enum TRAJ_FTW, /* Go to the dispenser. */ TRAJ_GTD, + /* Go to position. */ + TRAJ_GOTO, /* Everything done. */ TRAJ_DONE, }; @@ -55,6 +60,12 @@ enum /** Traj mode. */ uint8_t traj_mode; +/** Epsilon, distance considered to be small enough. */ +int16_t traj_eps = 500; + +/** Go to position. */ +static uint32_t traj_goto_x, traj_goto_y; + /** Angle offset. Directly handled to speed layer. */ void traj_angle_offset_start (int32_t angle, uint8_t seq) @@ -146,6 +157,56 @@ traj_gtd_start (uint8_t seq) state_start (&state_main, seq); } +/** Go to position mode. */ +static void +traj_goto (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, stop. */ + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } + else + { + /* Projection of destination in robot base. */ + int32_t c = fixed_cos_f824 (postrack_a); + int32_t s = fixed_sin_f824 (postrack_a); + int32_t dt = fixed_mul_f824 (dx, c) + fixed_mul_f824 (dy, s); + int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s); + /* Compute arc length. */ + int32_t arad = atan2 (da, dt) * (1L << 24); + 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)) + { + speed_theta.pos_cons = pos_theta.cur; + speed_theta.pos_cons += dt >> 8; + } + else + { + speed_theta.pos_cons = pos_theta.cons; + } + } +} + +/** Start go to position mode (x, y: f24.8). */ +void +traj_goto_start (uint32_t x, uint32_t y, uint8_t seq) +{ + state_main.mode = MODE_TRAJ; + traj_mode = TRAJ_GOTO; + traj_goto_x = x; + traj_goto_y = y; + speed_theta.use_pos = speed_alpha.use_pos = 1; + state_start (&state_main, seq); +} + /* Compute new speed according the defined trajectory. */ void traj_update (void) @@ -158,6 +219,9 @@ traj_update (void) case TRAJ_GTD: traj_gtd (); break; + case TRAJ_GOTO: + traj_goto (); + break; case TRAJ_DONE: break; } diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index d80460bf..c13eab9d 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -26,6 +26,7 @@ * }}} */ extern uint8_t traj_mode; +extern int16_t traj_eps; void traj_update (void); @@ -39,4 +40,7 @@ traj_ftw_start (uint8_t seq); void traj_gtd_start (uint8_t seq); +void +traj_goto_start (uint32_t x, uint32_t y, 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 e977bf76..75a0a110 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -139,6 +139,14 @@ twi_proto_callback (u8 *buf, u8 size) /* Go to the dispenser. */ traj_gtd_start (0); break; + case c ('x', 0): + /* Go to position. + * - 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), + 0); + break; case c ('b', 3): /* Move the arm. * - w: new position. -- cgit v1.2.3