summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/counter.c3
-rw-r--r--n/asserv/src/main.c1
-rw-r--r--n/asserv/src/postrack.c41
-rw-r--r--n/asserv/src/pwm.c13
-rw-r--r--n/asserv/src/test_pwm.c1
-rw-r--r--n/asserv/src/timer.c3
6 files changed, 39 insertions, 23 deletions
diff --git a/n/asserv/src/counter.c b/n/asserv/src/counter.c
index cfdf2e6..46dc05a 100644
--- a/n/asserv/src/counter.c
+++ b/n/asserv/src/counter.c
@@ -30,7 +30,8 @@ static uint8_t counter_left_frw, counter_left_rev,
static uint8_t counter_left_old, counter_right_old;
/** Overall counter values. */
static uint16_t counter_left, counter_right;
-/** Counter differences since last update. */
+/** Counter differences since last update.
+ * Maximum of 9 significant bits, sign included. */
static int16_t counter_left_diff, counter_right_diff;
/* +AutoDec */
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 2adbb76..72d4b39 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -146,6 +146,7 @@ main_loop (void)
counter_left >> 8, counter_left,
counter_right >> 8, counter_right);
motor_stat_counter_cpt = motor_stat_counter;
+ }
if (motor_stat_postrack && !--motor_stat_postrack_cpt)
{
proto_send4 ('X',
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;
}
diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c
index 7979842..883201f 100644
--- a/n/asserv/src/pwm.c
+++ b/n/asserv/src/pwm.c
@@ -46,14 +46,14 @@ pwm_update (void);
static inline void
pwm_init (void)
{
- /* No timer/counter interrupt. */
- //TIMSK = 0;
- //ETIMSK = 0;
/* Phase correct PWM, TOP = 0xff, OC1B & OC1C with positive logic.
f_IO without prescaler.
Fpwm = f_IO / (2 * prescaler * TOP) = 28912 Hz. */
- TCCR1A = _BV (COM1B1) | _BV (COM1C1) | _BV (WGM10);
- TCCR1B = _BV (CS10);
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 0, 0, 1, 0, 1, 0, 0, 1);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 0, 0, 0, 1);
/* Enable pwm and direction outputs in DDRB. */
DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2);
}
@@ -62,7 +62,8 @@ pwm_init (void)
static inline uint8_t
pwm_preproc (uint16_t v)
{
- v += 0x10;
+ // This is a try to correct the optocoupler problem...
+ //v += 0x10;
if (v > 255)
return 255;
else
diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c
index 96e2d81..37d6adf 100644
--- a/n/asserv/src/test_pwm.c
+++ b/n/asserv/src/test_pwm.c
@@ -24,6 +24,7 @@
* }}} */
#include <n/avr/rs232/rs232.h>
#include <n/avr/proto/proto.h>
+#include <n/avr/utils/utils.h>
#include <avr/io.h>
/* +AutoDec */
diff --git a/n/asserv/src/timer.c b/n/asserv/src/timer.c
index 1e18aca..42b9a68 100644
--- a/n/asserv/src/timer.c
+++ b/n/asserv/src/timer.c
@@ -43,10 +43,11 @@ timer_read (void);
static inline void
timer_init (void)
{
- /* 1024 prescaler. */
+ /* 256 prescaler. */
TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00,
0, 0, 0, 0, 0, 1, 1, 0);
/* Fov = F_io / (prescaler * (TOP + 1))
+ * TOP = 0xff
* Tov = 1 / Fov = 4.44 ms */
}