From dc5e9a8ab529223d5c32e5d803b4516a28da9586 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 28 Nov 2004 21:03:15 +0000 Subject: Test du calcul de position et modification pour les grandes vitesses (non testé). Modification de commentaires, vérification d'exactitude de calcul, notament sur les nombres de bits, prise en compte de la vitesse max des moteurs. --- n/asserv/src/postrack.c | 41 ++++++++++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 15 deletions(-) (limited to 'n/asserv/src/postrack.c') diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c index 0b6d196..5af427e 100644 --- a/n/asserv/src/postrack.c +++ b/n/asserv/src/postrack.c @@ -30,7 +30,14 @@ int32_t postrack_a; /** Distance between the weels, u16. */ uint16_t postrack_footing; /** Precomputed footing factor, f8.24. - * postrack_footing_factor = 1 / postrack_footing */ + * postrack_footing_factor = (1/2pi * 256) / postrack_footing + * Explanations: + * - Angles are between 0 and 1, corresponding to 0 and 2pi, therefore we + * must divide by 2pi to convert unit (Arc=Angle * Radius only works for + * radians). + * - dd (see postrack_update) is in f11.16 format, we multiply by 256 to have + * a angle in 8.24 format. + * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). */ int32_t postrack_footing_factor; /* +AutoDec */ @@ -53,7 +60,7 @@ postrack_set_footing (uint16_t footing); static inline void postrack_init (void) { - postrack_set_footing (5000); + postrack_set_footing (5142); } /** Update the current position. */ @@ -61,34 +68,38 @@ static inline void postrack_update (void) { int32_t d, dd, da, r, na; - d = counter_right_diff + counter_left_diff; - d <<= 24 - 1; + d = counter_right_diff + counter_left_diff; /* 10b */ + d <<= 16; /* 10.16b */ + d >>= 1; /* 9.16b */ if (counter_right_diff == counter_left_diff) { - postrack_x += dsp_mul_f824 (d, dsp_cos (postrack_a)) >> 16; - postrack_y += dsp_mul_f824 (d, dsp_sin (postrack_a)) >> 16; + postrack_x += dsp_mul_f824 (d, dsp_cos (postrack_a)) >> 8; + postrack_y += dsp_mul_f824 (d, dsp_sin (postrack_a)) >> 8; } else { - /* XXX: WARNING: dleft & dright 16 bits? - * This code should not work at high speed (> 127). */ - dd = counter_right_diff - counter_left_diff; - dd <<= 24; - da = dsp_mul_f824 (postrack_footing_factor, dd); - r = dsp_div_f824 (d, da); + dd = counter_right_diff - counter_left_diff; /* 11b */ + dd <<= 16; /* 11.16b */ + da = dsp_mul_f824 (dd, postrack_footing_factor); + // XXX: WARNING ! could r overflow ? + r = dsp_div_f824 (d, da); /* 16.16b */ na = postrack_a + da; postrack_x += - dsp_mul_f824 (r, dsp_sin (na) - dsp_sin (postrack_a)) >> 16; + dsp_mul_f824 (r, dsp_sin (na) - dsp_sin (postrack_a)) >> 8; postrack_y += - dsp_mul_f824 (r, dsp_cos (postrack_a) - dsp_cos (na)) >> 16; + dsp_mul_f824 (r, dsp_cos (postrack_a) - dsp_cos (na)) >> 8; postrack_a = na; + postrack_a &= 0x00ffffff; } } +#define M_1_PI 0.31830988618379067154 /* 1/pi */ + /** Change the footing value. */ static inline void postrack_set_footing (uint16_t footing) { postrack_footing = footing; - postrack_footing_factor = (1L << 24) / footing; + postrack_footing_factor = + (int32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing; } -- cgit v1.2.3