summaryrefslogtreecommitdiff
path: root/n/asserv/src/postrack.c
diff options
context:
space:
mode:
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;
}