From a46c224de1a250472b476eac28f1854a689d8b11 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 27 Mar 2005 18:44:56 +0000 Subject: Renomage de fonctions DSP. Nouveaux modes de déplacement. Grands travaux sur le protocol. --- n/asserv/src/avrconfig.h | 2 ++ n/asserv/src/dsp.c | 39 +++++++++++++++++++--- n/asserv/src/goto.c | 85 ++++++++++++++++++++++++++++++++++++++++++++---- n/asserv/src/main.c | 81 +++++++++++++++++++++++++++++++++------------ n/asserv/src/postrack.c | 8 ++--- n/asserv/src/speed.c | 4 +-- n/asserv/src/test_dsp.c | 10 +++--- 7 files changed, 186 insertions(+), 43 deletions(-) (limited to 'n/asserv') diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h index 769257c..be35d22 100644 --- a/n/asserv/src/avrconfig.h +++ b/n/asserv/src/avrconfig.h @@ -60,5 +60,7 @@ #define AC_PROTO_CALLBACK proto_callback /** Putchar function name. */ #define AC_PROTO_PUTC rs232_putc +/** Support for quote parameter. */ +#define AC_PROTO_QUOTE 1 #endif /* avrconfig_h */ diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c index 5bbcf65..2c6b21f 100644 --- a/n/asserv/src/dsp.c +++ b/n/asserv/src/dsp.c @@ -39,7 +39,7 @@ * * Angles are mapped from [0, 2pi) to [0,1). */ -/** Add two signed words (i16) and saturate. UNTESTED. */ +/** Add two signed words (i16) and saturate. UNUSED and UNTESTED. */ extern inline int16_t dsp_add_sat_i16i16 (int16_t a, int16_t b) { @@ -315,7 +315,7 @@ dsp_cos_dli (int32_t a) /** Compute cosinus, angle f8.24, result f8.24. */ int32_t -dsp_cos (int32_t a) +dsp_cos_f824 (int32_t a) { a &= (1L << 24) - 1; uint8_t z = ((uint32_t) a >> 16) & 0xc0; @@ -331,7 +331,7 @@ dsp_cos (int32_t a) /** Compute sinus, angle f8.24, result f8.24. */ int32_t -dsp_sin (int32_t a) +dsp_sin_f824 (int32_t a) { a &= (1L << 24) - 1; uint8_t z = ((uint32_t) a >> 16) & 0xc0; @@ -347,7 +347,7 @@ dsp_sin (int32_t a) /** Compute square root, uf24.8. */ uint32_t -dsp_sqrt (uint32_t x) +dsp_sqrt_f248 (uint32_t x) { uint32_t root, bit, test; root = 0; @@ -368,3 +368,34 @@ dsp_sqrt (uint32_t x) return root << 4; } +/** Compute square root, ui32 -> ui16. */ +uint16_t +dsp_sqrt_ui32 (uint32_t x) +{ + uint32_t root, bit, test; + root = 0; + bit = 1L << 30; + do + { + test = root + bit; + //printf ("test = 0x%x, root = 0x%x, bit = 0x%x\n", test, root, bit); + if (x >= test) + { + x -= test; + root = test + bit; + //printf ("x = 0x%x, root = 0x%x\n", x, root); + } + root >>= 1; + bit >>= 2; + } while (bit); + return root; +} + +/** Compute hypothenuse, f24.8 -> ui16. Input limited to +-65535 */ +uint16_t +dsp_hypot (int32_t dx, int32_t dy) +{ + dx >>= 8; + dy >>= 8; + return dsp_sqrt_ui32 (dx * dx + dy * dy); +} diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index dc8cc85..bd57d85 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -23,10 +23,20 @@ * * }}} */ +/** Goto mode: + * 0: linear move. + * 1: angular move. + * 2: position move. + */ +uint8_t goto_mode; +/** Distance for mode 0, ui16. */ +uint16_t goto_d; +/** Sign of movement (0: forward, 1 backward). */ +uint8_t goto_sign; /** Destination position, f24.8. */ int32_t goto_x, goto_y; -/** Destination angle, f8.24. */ -int32_t goto_a; +/** Destination angle, f0.8. */ +int8_t goto_a; /** Destination epsillon. */ int32_t goto_eps = 20L << 8; /** Debug values. */ @@ -35,9 +45,52 @@ int32_t goto_dx, goto_dy, goto_dl, goto_da; /* +AutoDec */ /* -AutoDec */ -/** Update the speed according to the desired destination. */ +/** Linear control mode. */ static inline void -goto_update (void) +goto_linear_mode (void) +{ + uint16_t d; + uint32_t dx, dy; + /* Compute distance from the start point. + * WARNING: this could overflow as dsp_hypot accept a maximum value of + * +/- 65535. */ + dx = goto_x - postrack_x; + dy = goto_y - postrack_y; + d = dsp_hypot (dx, dy); + /* Change speed. */ + if (d > goto_d) + { + speed_left_aim = 0; + speed_right_aim = 0; + } + else + { + /* Convert back to f24.8. */ + int32_t com = goto_d - d; + com <<= 8; + speed_distance (goto_sign ? -com : com, 0); + } +} + +/** Angular control mode. */ +static inline void +goto_angular_mode (void) +{ + int8_t angle_diff; + int32_t arc; + /* Compute angle diff. This works because a full circle is 256. */ + angle_diff = goto_a - v32_to_v8 (postrack_a, 2); + /* Compute arc. */ + arc = angle_diff; /* i8 */ + arc *= postrack_footing / 2; /* i24 */ + arc <<= 8; /* f24.8 */ + /* Change speed. */ + speed_distance (0, arc); +} + +/** Position control mode. */ +static inline void +goto_position_mode (void) { int32_t c, s; /* Project in the robot base. */ @@ -51,13 +104,31 @@ goto_update (void) } else { - c = dsp_cos (postrack_a); - s = dsp_sin (postrack_a); + c = dsp_cos_f824 (postrack_a); + s = dsp_sin_f824 (postrack_a); goto_dl = dsp_mul_f824 (goto_dx, c) + dsp_mul_f824 (goto_dy, s); goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s); /* Convert da into a arc. This is a rough aproximation. */ - //da = da * postrack_footing / 2 / (dl >> 8); + goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8); speed_distance (goto_dl, goto_da); } } +/** Update the speed according to the desired destination. */ +static inline void +goto_update (void) +{ + switch (goto_mode) + { + case 0: + goto_linear_mode (); + break; + case 1: + goto_angular_mode (); + break; + case 2: + goto_position_mode (); + break; + } +} + diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 2d21804..d119e4b 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -38,8 +38,8 @@ #include "goto.c" /** Motor mode : - * 0 - pwm setup; - * 1 - speed control; + * 0 - pwm setup. + * 1 - speed control. * 2 - position control. */ int8_t motor_mode; @@ -180,8 +180,23 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) reset (); break; /* Commands. */ + case c ('l', 2): + motor_mode = 2; + goto_mode = 0; + goto_sign = args[0] >> 7; + goto_d = v8_to_v16 (args[0], args[1]); + if (goto_sign) goto_d = -goto_d; + goto_x = postrack_x; + goto_y = postrack_y; + break; + case c ('a', 1): + motor_mode = 2; + goto_mode = 1; + goto_a = args[0]; + break; case c ('g', 8): motor_mode = 2; + goto_mode = 2; goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]); goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]); break; @@ -199,7 +214,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) motor_mode = 1; speed_left_aim = args[0]; speed_right_aim = args[1]; - speed_max = args[2]; break; case c ('w', 0): speed_restart (); @@ -213,22 +227,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_left = v8_to_v16 (args[0], args[1]); pwm_right = v8_to_v16 (args[2], args[3]); break; - /* Params. */ - case c ('e', 4): - goto_eps = v8_to_v32 (args[0], args[1], args[2], args[3]); - break; - case c ('a', 1): - speed_acc_cpt = speed_acc = args[0]; - break; - case c ('p', 2): - speed_kp = v8_to_v16 (args[0], args[1]); - break; - case c ('i', 2): - speed_ki = v8_to_v16 (args[0], args[1]); - break; - case c ('f', 2): - postrack_set_footing (v8_to_v16 (args[0], args[1])); - break; /* Stats. */ case c ('C', 1): motor_stat_counter_cpt = motor_stat_counter = args[0]; @@ -249,8 +247,49 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) motor_stat_timer_cpt = motor_stat_timer = args[0]; break; default: - proto_send0 ('e'); - return; + /* Params. */ + if (cmd == 'p') + { + switch (c (args[0], size)) + { + case c ('x', 5): + postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('y', 5): + postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('a', 5): + postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('f', 3): + postrack_set_footing (v8_to_v16 (args[1], args[2])); + break; + case c ('e', 5): + goto_eps = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('p', 3): + speed_kp = v8_to_v16 (args[1], args[2]); + break; + case c ('i', 3): + speed_ki = v8_to_v16 (args[1], args[2]); + break; + case c ('a', 2): + speed_acc_cpt = speed_acc = args[1]; + break; + case c ('m', 2): + speed_max = args[1]; + break; + default: + proto_send0 ('e'); + return; + } + } + else + { + proto_send0 ('e'); + return; + } + break; } proto_send (cmd, size, args); #undef c diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c index d7e6bc8..ae0d0c3 100644 --- a/n/asserv/src/postrack.c +++ b/n/asserv/src/postrack.c @@ -76,8 +76,8 @@ postrack_update (void) if (counter_right_diff == counter_left_diff) { /* Line. */ - postrack_x += dsp_mul_f824 (d, dsp_cos (postrack_a)) >> 8; - postrack_y += dsp_mul_f824 (d, dsp_sin (postrack_a)) >> 8; + postrack_x += dsp_mul_f824 (d, dsp_cos_f824 (postrack_a)) >> 8; + postrack_y += dsp_mul_f824 (d, dsp_sin_f824 (postrack_a)) >> 8; } else { @@ -90,11 +90,11 @@ postrack_update (void) /* Compute da in radians. */ da = dsp_mul_f824 (da, 2 * M_PI * (1L << 24)); /* X increment. */ - dsc = dsp_sin (na) - dsp_sin (postrack_a); /* 8.24b */ + dsc = dsp_sin_f824 (na) - dsp_sin_f824 (postrack_a); /* 8.24b */ dsc = dsp_div_f824 (dsc, da); /* 8.24b < 1 */ postrack_x += dsp_mul_f824 (d, dsc) >> 8; /* 24.8b */ /* Y increment. */ - dsc = dsp_cos (postrack_a) - dsp_cos (na); /* 8.24b */ + dsc = dsp_cos_f824 (postrack_a) - dsp_cos_f824 (na); /* 8.24b */ dsc = dsp_div_f824 (dsc, da); /* 8.24b < 1 */ postrack_y += dsp_mul_f824 (d, dsc) >> 8; /* 24.8b */ /* Angle update. */ diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c index 3b9a709..38842a1 100644 --- a/n/asserv/src/speed.c +++ b/n/asserv/src/speed.c @@ -177,9 +177,9 @@ speed_distance (int32_t dist, int32_t arc) if (vas) arc = -arc; /* Compute speed. * v = sqrt (2 * a * d), a = 1/speed_acc */ - vl248 = dsp_sqrt (dist * 2 / speed_acc); + vl248 = dsp_sqrt_f248 (dist * 2 / speed_acc); vl = (vl248 >> 8) & 0xff; - va248 = dsp_sqrt (arc * 2 / speed_acc); + va248 = dsp_sqrt_f248 (arc * 2 / speed_acc); va = (va248 >> 8) & 0xff; /* Saturate. */ if (vl248 & 0xffff0000 || vl > speed_max) diff --git a/n/asserv/src/test_dsp.c b/n/asserv/src/test_dsp.c index b1a0c9b..7016954 100644 --- a/n/asserv/src/test_dsp.c +++ b/n/asserv/src/test_dsp.c @@ -79,9 +79,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) proto_send1d ('r', rl); break; case 'c' | 1 << 8: - rl = dsp_cos (sa); + rl = dsp_cos_f824 (sa); proto_send1d ('r', rl); - rl = dsp_sin (sa); + rl = dsp_sin_f824 (sa); proto_send1d ('r', rl); break; case 'M' | 0 << 8: @@ -134,9 +134,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) for (al = 0; al < (1L << 24) + (1L << 21); al += 32 << 8) { proto_send1d ('c', al); - rl = dsp_cos (al); + rl = dsp_cos_f824 (al); proto_send1d ('r', rl); - rl = dsp_sin (al); + rl = dsp_sin_f824 (al); proto_send1d ('r', rl); } break; @@ -147,7 +147,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) { al = wl[k] >> i; proto_send1d ('s', al); - rl = dsp_sqrt (al); + rl = dsp_sqrt_f248 (al); proto_send1d ('r', rl); } } -- cgit v1.2.3