From 714e86c96ff93fb78d4340cb607c12927091417b Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 1 May 2005 15:36:43 +0000 Subject: 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. --- n/asserv/src/eeprom.c | 14 +++++--- n/asserv/src/main.c | 27 +++++++++++++- n/asserv/src/postrack.c | 6 +--- n/asserv/src/pwm.c | 10 +++--- n/asserv/src/robots.txt | 35 ++++++++++++++++++ n/asserv/src/speed.c | 95 ++++++++++++++++++++++++++++++++----------------- n/asserv/src/taz.c | 14 ++++---- n/asserv/src/timer.c | 6 ++-- 8 files changed, 150 insertions(+), 57 deletions(-) create mode 100644 n/asserv/src/robots.txt (limited to 'n/asserv') 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 +#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. */ -- cgit v1.2.3