summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2005-05-01 15:36:43 +0000
committerschodet2005-05-01 15:36:43 +0000
commit714e86c96ff93fb78d4340cb607c12927091417b (patch)
treef28de78f252dc450550151c6324f1e44b4633473
parent7bed3836db97e3c626a604cbe8b10c49236bb724 (diff)
Ajout de e_sat.
Ajout de moyenage de d. Ajout d'un print des paramètres. Mesures des robot. Période d'asservissement 1.1 ms. PWM à 14KHz.
-rw-r--r--n/asserv/src/eeprom.c14
-rw-r--r--n/asserv/src/main.c27
-rw-r--r--n/asserv/src/postrack.c6
-rw-r--r--n/asserv/src/pwm.c10
-rw-r--r--n/asserv/src/robots.txt35
-rw-r--r--n/asserv/src/speed.c95
-rw-r--r--n/asserv/src/taz.c14
-rw-r--r--n/asserv/src/timer.c6
8 files changed, 150 insertions, 57 deletions
diff --git a/n/asserv/src/eeprom.c b/n/asserv/src/eeprom.c
index 3eb0140..4cd3a10 100644
--- a/n/asserv/src/eeprom.c
+++ b/n/asserv/src/eeprom.c
@@ -23,6 +23,8 @@
* }}} */
#include <avr/eeprom.h>
+#define EEPROM_KEY 0xa7
+
/* +AutoDec */
/* -AutoDec */
@@ -32,16 +34,18 @@ eeprom_read_params (void)
{
uint8_t *p8 = 0;
uint16_t *p16;
- if (eeprom_read_byte (p8++) != 0xa5)
+ if (eeprom_read_byte (p8++) != EEPROM_KEY)
return;
speed_acc_cpt = speed_acc = eeprom_read_byte (p8++);
speed_max = eeprom_read_byte (p8++);
+ speed_ds = eeprom_read_byte (p8++);
pwm_dir = eeprom_read_byte (p8++);
p16 = (uint16_t *) p8;
speed_kp = eeprom_read_word (p16++);
speed_ki = eeprom_read_word (p16++);
speed_kd = eeprom_read_word (p16++);
- speed_int_max = eeprom_read_word (p16++);
+ speed_e_sat = eeprom_read_word (p16++);
+ speed_int_sat = eeprom_read_word (p16++);
postrack_set_footing (eeprom_read_word (p16++));
}
@@ -51,15 +55,17 @@ eeprom_write_params (void)
{
uint8_t *p8 = 0;
uint16_t *p16;
- eeprom_write_byte (p8++, 0xa5);
+ eeprom_write_byte (p8++, EEPROM_KEY);
eeprom_write_byte (p8++, speed_acc);
eeprom_write_byte (p8++, speed_max);
+ eeprom_write_byte (p8++, speed_ds);
eeprom_write_byte (p8++, pwm_dir);
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, speed_kp);
eeprom_write_word (p16++, speed_ki);
eeprom_write_word (p16++, speed_kd);
- eeprom_write_word (p16++, speed_int_max);
+ eeprom_write_word (p16++, speed_e_sat);
+ eeprom_write_word (p16++, speed_int_sat);
eeprom_write_word (p16++, postrack_footing);
}
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index fe99acf..e6c2303 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -339,6 +339,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
switch (c (args[0], size))
{
+ case c ('x', 1):
+ postrack_x = 0;
+ postrack_y = 0;
+ postrack_a = 0;
+ break;
case c ('x', 5):
/* Set current x position. */
postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]);
@@ -368,8 +373,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('d', 3):
speed_kd = v8_to_v16 (args[1], args[2]);
break;
+ case c ('E', 3):
+ speed_e_sat = v8_to_v16 (args[1], args[2]);
+ break;
case c ('I', 3):
- speed_int_max = v8_to_v16 (args[1], args[2]);
+ speed_int_sat = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('s', 2):
+ speed_ds = args[1];
break;
case c ('a', 2):
/* Set acceleration. */
@@ -395,6 +406,20 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
else
eeprom_clear_params ();
break;
+ case c ('P', 1):
+ /* Print current settings. */
+ proto_send1b ('E', EEPROM_KEY);
+ proto_send1w ('f', postrack_footing);
+ proto_send1b ('a', speed_acc);
+ proto_send1b ('m', speed_max);
+ proto_send1w ('p', speed_kp);
+ proto_send1w ('i', speed_ki);
+ proto_send1w ('d', speed_kd);
+ proto_send1w ('E', speed_e_sat);
+ proto_send1w ('I', speed_int_sat);
+ proto_send1b ('s', speed_ds);
+ proto_send1b ('w', pwm_dir);
+ break;
default:
proto_send0 ('?');
return;
diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c
index 197cc67..6478824 100644
--- a/n/asserv/src/postrack.c
+++ b/n/asserv/src/postrack.c
@@ -62,11 +62,7 @@ postrack_set_footing (uint16_t footing);
static inline void
postrack_init (void)
{
- // Mesured : 2667.
- // Correction : / 1.00708 -> 6223 (184F).
- // Correction : / 1.00210 -> 6209.9107 (1842).
- // Correction : -> 6216 (1848).
- postrack_set_footing (0x1842);
+ postrack_set_footing (0x184c);
}
#define M_PI 3.14159265358979323846 /* pi */
diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c
index 4ab8722..a200213 100644
--- a/n/asserv/src/pwm.c
+++ b/n/asserv/src/pwm.c
@@ -44,7 +44,7 @@ static inline void
pwm_init (void);
/** Preprocess a PWM value before sending it to hardware. */
-static inline uint8_t
+static inline uint16_t
pwm_preproc (uint16_t v);
/** Update the hardware PWM values. */
@@ -62,7 +62,7 @@ pwm_init (void)
Fpwm = f_IO / (2 * prescaler * TOP) = 28912 Hz. */
TCCR1A =
regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
- 0, 0, 1, 0, 1, 0, 0, 1);
+ 0, 0, 1, 0, 1, 0, 1, 0);
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. */
@@ -70,11 +70,11 @@ pwm_init (void)
}
/** Preprocess a PWM value before sending it to hardware. */
-static inline uint8_t
+static inline uint16_t
pwm_preproc (uint16_t v)
{
- if (v > 255)
- return 255;
+ if (v > 511)
+ return 511;
else
return v;
}
diff --git a/n/asserv/src/robots.txt b/n/asserv/src/robots.txt
new file mode 100644
index 0000000..f82dba0
--- /dev/null
+++ b/n/asserv/src/robots.txt
@@ -0,0 +1,35 @@
+kp * E: 15b
+ki * I: 14b
+kd * E: ~13b
+
+-----------------------------
+Mani:
+
+kp 16 0fff
+E 1023 03ff
+ki 1/4 0040
+I 8191 1fff
+kd 16 0fff
+ds 1 01
+a 64 40
+m 16 10
+scale .05076697072187489323
+footing 315.861 E3F.29
+
+20 tours: 37F70 37EDC
+
+-----------------------------
+Taz:
+
+kp 16 0fff
+E 1023 03ff
+ki 1/32 0008
+I 8191 1fff
+kd 16 0fff
+ds 2 02
+a 64 40
+m 16 10
+scale .05077874252508530693
+footing 185.198 184C.59
+
+20 tours: 5F783 5F5E5
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 8050b29..06b3abc 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -27,24 +27,34 @@
int8_t speed_left, speed_right;
/** Wanted speed. */
int8_t speed_left_aim, speed_right_aim;
-/** Max speed. */
-int8_t speed_max = 0x20;
+/** Max speed for automatic movements. */
+int8_t speed_max = 0x10;
/** Acceleration value. */
-uint8_t speed_acc = 8;
+uint8_t speed_acc = 0x40;
/** Acceleration counter, speed gets updated when it reachs 0. */
uint8_t speed_acc_cpt;
+/** Error saturation. */
+int16_t speed_e_sat = 1023;
+/** Integral saturation. */
+int16_t speed_int_sat = 8191;
+/** P coeficients. 4.8 fixed point format. */
+uint16_t speed_kp = 0x0fff;
+/** I coeficients. 3.8 fixed point format. */
+uint16_t speed_ki = 0x0008;
+/** D coeficients. 3.8 fixed point format. */
+uint16_t speed_kd = 0x0fff;
+/** D sample period. */
+uint8_t speed_ds = 2;
/** Integral term. */
int16_t speed_left_int, speed_right_int;
-/** Integral max value. 1024 maximum. */
-int16_t speed_int_max = 1024;
/** Last error value. */
int16_t speed_left_e_old, speed_right_e_old;
-/** P coeficients. 4.8 fixed point format. */
-uint16_t speed_kp = 0x0200;
-/** I coeficients. 3.8 fixed point format. */
-uint16_t speed_ki = 0x0004;
-/** D coeficients. 3.8 fixed point format. */
-uint16_t speed_kd = 0x0100;
+/** D history. */
+int16_t speed_left_d_hist[16], speed_right_d_hist[16];
+/** D history pointer. */
+uint8_t speed_d_hist_ptr;
+/** D history sum. */
+int16_t speed_left_d_hist_sum, speed_right_d_hist_sum;
/** Counter value wanted. */
uint16_t speed_left_counter, speed_right_counter;
/** Do not update counter. */
@@ -126,22 +136,28 @@ speed_compute_left_pwm (void)
int16_t e, diff, pwm;
/* Get and saturate error. */
e = speed_left_counter - counter_left; /* 11b */
- if (e > 1024)
- e = 1024;
- else if (e < -1024)
- e = -1024;
+ if (e > speed_e_sat)
+ e = speed_e_sat;
+ else if (e < -speed_e_sat)
+ e = -speed_e_sat;
/* Integral update. */
speed_left_int += e; /* 12b = 11b + 10b */
- if (speed_left_int > speed_int_max) /* 11b */
- speed_left_int = speed_int_max;
- else if (speed_left_int < -speed_int_max)
- speed_left_int = -speed_int_max;
+ if (speed_left_int > speed_int_sat) /* 11b */
+ speed_left_int = speed_int_sat;
+ else if (speed_left_int < -speed_int_sat)
+ speed_left_int = -speed_int_sat;
/* Differential value. */
diff = e - speed_left_e_old; /* 11b */
+ speed_left_d_hist_sum += diff;
+ speed_left_d_hist_sum -= speed_left_d_hist[speed_d_hist_ptr];
+ speed_left_d_hist[speed_d_hist_ptr] = diff;
+ speed_d_hist_ptr++;
+ if (speed_d_hist_ptr >= speed_ds)
+ speed_d_hist_ptr = 0;
/* Compute PI. */ /* 16b = 15b + 14b + 14b */
- pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 11b * 4.8b */
- + dsp_mul_i16f88 (speed_left_int, speed_ki) /* 14b = 11b * 3.8b */
- + dsp_mul_i16f88 (diff, speed_kd); /* 14b = 11b * 3.8b */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b */
+ + dsp_mul_i16f88 (speed_left_int, speed_ki) /* 14b */
+ + dsp_mul_i16f88 (speed_left_d_hist_sum, speed_kd); /* 14b */
/* Save result. */
speed_left_e_old = e;
pwm_left = pwm;
@@ -154,22 +170,25 @@ speed_compute_right_pwm (void)
int16_t e, diff, pwm;
/* Get and saturate error. */
e = speed_right_counter - counter_right; /* 11b */
- if (e > 1024)
- e = 1024;
- else if (e < -1024)
- e = -1024;
+ if (e > speed_e_sat)
+ e = speed_e_sat;
+ else if (e < -speed_e_sat)
+ e = -speed_e_sat;
/* Integral update. */
speed_right_int += e; /* 12b = 11b + 10b */
- if (speed_right_int > speed_int_max) /* 11b */
- speed_right_int = speed_int_max;
- else if (speed_right_int < -speed_int_max)
- speed_right_int = -speed_int_max;
+ if (speed_right_int > speed_int_sat) /* 11b */
+ speed_right_int = speed_int_sat;
+ else if (speed_right_int < -speed_int_sat)
+ speed_right_int = -speed_int_sat;
/* Differential value. */
diff = e - speed_right_e_old; /* 11b */
+ speed_right_d_hist_sum += diff;
+ speed_right_d_hist_sum -= speed_right_d_hist[speed_d_hist_ptr];
+ speed_right_d_hist[speed_d_hist_ptr] = diff;
/* Compute PI. */ /* 16b = 15b + 14b + 14b */
- pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 11b * 4.8b */
- + dsp_mul_i16f88 (speed_right_int, speed_ki) /* 14b = 11b * 3.8b */
- + dsp_mul_i16f88 (diff, speed_kd); /* 14b = 11b * 3.8b */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b */
+ + dsp_mul_i16f88 (speed_right_int, speed_ki) /* 14b */
+ + dsp_mul_i16f88 (speed_right_d_hist_sum, speed_kd); /* 14b */
/* Save result. */
speed_right_e_old = e;
pwm_right = pwm;
@@ -180,12 +199,22 @@ speed_compute_right_pwm (void)
static inline void
speed_restart (void)
{
+ int i;
speed_left_int = 0;
speed_right_int = 0;
speed_left = 0;
speed_right = 0;
speed_left_counter = counter_left;
speed_right_counter = counter_right;
+ speed_left_d_hist_sum = 0;
+ speed_right_d_hist_sum = 0;
+ speed_d_hist_ptr = 0;
+ for (i = 0; i < sizeof (speed_left_d_hist)
+ / sizeof (speed_left_d_hist[0]); i++)
+ {
+ speed_left_d_hist[i] = 0;
+ speed_right_d_hist[i] = 0;
+ }
}
/** Set maximum speed in order to be able to break before a distance and
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
index 8aba939..60c8330 100644
--- a/n/asserv/src/taz.c
+++ b/n/asserv/src/taz.c
@@ -32,10 +32,12 @@ uint8_t taz_max_state = 0xff;
uint8_t taz_max_substate;
/** Positions. */
-//#define taz_scale (1.0/0.05026548245743669181)
-//#define taz_scale (1.0/.05090591755319148936)
-//#define taz_scale (1.0/.05132374757503138194)
-#define taz_scale (1.0/.05124525981346725427)
+/********************************
+ * *
+ * !!! LES Y SONT NÉGATIFS !!! *
+ * *
+ ********************************/
+#define taz_scale (1.0/.05077874252508530693)
static const uint32_t taz_rear_angle = 0x00ff90bf;
static const uint16_t taz_rear_16 = taz_scale * 270 / 2;
static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256;
@@ -229,13 +231,13 @@ taz_state_1 (void)
taz_substate = 2;
break;
}
- /* Direction l'axe des y. */
if (taz_pont == 0)
{
taz_substate = 10;
}
else
{
+ /* Direction l'axe des y. */
taz_substate = 6;
motor_mode = 2;
goto_mode = 1;
@@ -273,7 +275,7 @@ taz_state_1 (void)
motor_mode = 2;
goto_mode = 0;
goto_sign = 0;
- goto_d = taz_start_y_16 + taz_pont * taz_brige_interval_16 -
+ goto_d = taz_start_y_16 + taz_pont * taz_brige_interval_16 +
(postrack_y >> 8);
goto_x = postrack_x;
goto_y = postrack_y;
diff --git a/n/asserv/src/timer.c b/n/asserv/src/timer.c
index 42b9a68..b6ee1f1 100644
--- a/n/asserv/src/timer.c
+++ b/n/asserv/src/timer.c
@@ -43,12 +43,12 @@ timer_read (void);
static inline void
timer_init (void)
{
- /* 256 prescaler. */
+ /* 64 prescaler. */
TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00,
- 0, 0, 0, 0, 0, 1, 1, 0);
+ 0, 0, 0, 0, 0, 1, 0, 0);
/* Fov = F_io / (prescaler * (TOP + 1))
* TOP = 0xff
- * Tov = 1 / Fov = 4.44 ms */
+ * Tov = 1 / Fov = 1.111 ms */
}
/** Test if a overflow occured and reset the flag. */