From 2cd194c92ebefa7f88c03a851854b0a8b1512413 Mon Sep 17 00:00:00 2001 From: schodet Date: Mon, 11 Apr 2005 23:08:44 +0000 Subject: Asservissement en position à la LM629. --- n/asserv/src/speed.c | 65 ++++++++++++++++++++++++++++++++-------------------- 1 file changed, 40 insertions(+), 25 deletions(-) (limited to 'n/asserv/src') diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c index 7fdb0dc..8c67207 100644 --- a/n/asserv/src/speed.c +++ b/n/asserv/src/speed.c @@ -35,16 +35,18 @@ uint8_t speed_acc = 8; uint8_t speed_acc_cpt; /** Integral term. */ int16_t speed_left_int, speed_right_int; -/** Integral max value. */ +/** 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. 5.8 fixed point format. */ +/** P coeficients. 4.8 fixed point format. */ uint16_t speed_kp = 5 * 255; -/** I coeficients. 4.8 fixed point format. */ +/** I coeficients. 3.8 fixed point format. */ uint16_t speed_ki = 1 * 255; -/** Dead zone. */ -uint16_t speed_dead_zone = 0; +/** D coeficients. 3.8 fixed point format. */ +uint16_t speed_kd = 1 * 255; +/** Counter value wanted. */ +uint16_t speed_left_counter, speed_right_counter; /* +AutoDec */ @@ -107,27 +109,34 @@ speed_update (void) speed_left = speed_left_aim; speed_right = speed_right_aim; } + /* Update counter. */ + speed_left_counter += speed_left; + speed_right_counter += speed_right; } /** Compute new pwm value for left motor. */ static inline void speed_compute_left_pwm (void) { - int16_t e; - int16_t pwm; - e = speed_left - counter_left_diff; /* 10b = 8b + 9b */ - /* Dead zone. */ - if (speed_dead_zone > e && e > -speed_dead_zone) - e = 0; + 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; /* 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; - /* Compute PI. */ /* 16b = 15b + 15b */ - pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 10b * 5.8b */ - + dsp_mul_i16f88 (speed_left_int, speed_ki); /* 15b = 11b * 4.8b */ + /* Differential value. */ + diff = e - speed_left_e_old; /* 11b */ + /* 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 */ /* Save result. */ speed_left_e_old = e; pwm_left = pwm; @@ -137,21 +146,25 @@ speed_compute_left_pwm (void) static inline void speed_compute_right_pwm (void) { - int16_t e; - int16_t pwm; - e = speed_right - counter_right_diff; - /* Dead zone. */ - if (speed_dead_zone > e && e > -speed_dead_zone) - e = 0; + 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; /* Integral update. */ - speed_right_int += e; - if (speed_right_int > speed_int_max) + 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; - /* Compute PI. */ - pwm = dsp_mul_i16f88 (e, speed_kp) - + dsp_mul_i16f88 (speed_right_int, speed_ki); + /* Differential value. */ + diff = e - speed_right_e_old; /* 11b */ + /* 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 */ /* Save result. */ speed_right_e_old = e; pwm_right = pwm; @@ -166,6 +179,8 @@ speed_restart (void) speed_right_int = 0; speed_left = 0; speed_right = 0; + speed_left_counter = counter_left; + speed_right_counter = counter_right; } /** Set maximum speed in order to be able to break before a distance and -- cgit v1.2.3