summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv')
-rw-r--r--n/asserv/src/postrack.c32
1 files changed, 20 insertions, 12 deletions
diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c
index 646a67b..5adcb37 100644
--- a/n/asserv/src/postrack.c
+++ b/n/asserv/src/postrack.c
@@ -63,34 +63,42 @@ postrack_init (void)
postrack_set_footing (5142);
}
+#define M_PI 3.14159265358979323846 /* pi */
+
/** Update the current position. */
static inline void
postrack_update (void)
{
- int32_t d, dd, da, r, na;
+ int32_t d, dd, da, na, dsc;
d = counter_right_diff + counter_left_diff; /* 10b */
d <<= 16; /* 10.16b */
d >>= 1; /* 9.16b */
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;
}
else
{
- dd = counter_right_diff - counter_left_diff; /* 11b */
- dd <<= 16; /* 11.16b */
+ /* Arc. */
+ dd = counter_right_diff - counter_left_diff; /* 10b */
+ dd <<= 16; /* 10.16b */
da = dsp_mul_f824 (dd, postrack_footing_factor);/* 8.24b */
- // XXX: WARNING ! could r overflow ? Yes :,-(
- // Should try too divide by da later, but this means two divisions
- // instead of one.
- // Also, this division is false because da is not radians !
- r = dsp_div_f824 (d, da); /* 16.16b */
+ // XXX: this division is false because da is not radians !
+ /* New angle. */
na = postrack_a + da;
- postrack_x +=
- 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)) >> 8;
+ /* 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_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_div_f824 (dsc, da); /* 8.24b < 1 */
+ postrack_y += dsp_mul_f824 (d, dsc) >> 8; /* 24.8b */
+ /* Angle update. */
postrack_a = na;
postrack_a &= 0x00ffffff;
}