summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2004-11-28 21:03:15 +0000
committerschodet2004-11-28 21:03:15 +0000
commitdc5e9a8ab529223d5c32e5d803b4516a28da9586 (patch)
tree3d47b4b80aa015e15274a3912cf04ce641b2b3f1
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.
-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 */
}