summaryrefslogtreecommitdiff
path: root/n/asserv/src/postrack.c
diff options
context:
space:
mode:
authorschodet2004-11-28 21:03:15 +0000
committerschodet2004-11-28 21:03:15 +0000
commitdc5e9a8ab529223d5c32e5d803b4516a28da9586 (patch)
tree3d47b4b80aa015e15274a3912cf04ce641b2b3f1 /n/asserv/src/postrack.c
parentfa8ff2dec76b1b366207a126dfc360331510c50d (diff)
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.
Diffstat (limited to 'n/asserv/src/postrack.c')
-rw-r--r--n/asserv/src/postrack.c41
1 files changed, 26 insertions, 15 deletions
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;
}