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/speed.c | 95 ++++++++++++++++++++++++++++++++++------------------ 1 file changed, 62 insertions(+), 33 deletions(-) (limited to 'n/asserv/src/speed.c') 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 -- cgit v1.2.3