From 1fcc21169ab15064ca0acbc0688d2f75a835a443 Mon Sep 17 00:00:00 2001 From: prot Date: Fri, 10 Dec 2004 02:01:05 +0000 Subject: Adding pid for linefol mode. (Still under work) --- n/line-follower/src/main.c | 9 +++++-- n/line-follower/src/speed.c | 57 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 64 insertions(+), 2 deletions(-) diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c index 72d4b39..f0059f9 100644 --- a/n/line-follower/src/main.c +++ b/n/line-follower/src/main.c @@ -38,7 +38,8 @@ /** Motor mode : * 0 - pwm setup; * 1 - speed control; - * 2 - position control. */ + * 2 - position control; + * 3 - linefol mode. */ int8_t motor_mode; /** Main loop counter. */ @@ -107,7 +108,11 @@ main_loop (void) postrack_update (); motor_timer_2 = timer_read (); /* Speed control. */ - if (motor_mode >= 1) + if (motor_mode = 3) + { + speed_compute_linefol_pwm (); + } + else if (motor_mode >= 1) { speed_update (); speed_compute_left_pwm (); diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c index 54962b3..a2f2c54 100644 --- a/n/line-follower/src/speed.c +++ b/n/line-follower/src/speed.c @@ -23,6 +23,14 @@ * * }}} */ +/** Maximum value the pwm can reach (defined by the avr configuration) */ +#define PWM_MAX 255 +/** Maximum value for the pwm in linefol mode (controls avg speed) */ +int16_t linefol_max_pwm = 80; +/** Table and index for calculating the derivate term */ +#define N_LINEPOS_DER_TAB 8 // max : 256 +int16_t linepos_der_tab[N_LINEPOS_DER_TAB]; +uint8_t linepos_der_index; /** Actual speed. */ int8_t speed_left, speed_right; /** Wanted speed. */ @@ -104,6 +112,55 @@ speed_update (void) speed_right = speed_right_aim; } } +/** Compute new pwm value for motors in linefol mode. */ +static inline void +speed_compute_linefol_pwm (void) +{ + int16_t e,speed_linepos_der; + uint8_t i; + int16_t pwm; + // reusing left channel variables + + e = linepos; /* 10b = 8b + 9b */ + + /* Derivative update. */ + i=linepos_der_index; // old index + linepos_der_index= // updating index by + (linepos_der_index+N_LINEPOS_DER_TAB-1) // modulo adressing + & (N_LINEPOS_DER_TAB-1); + linepos_der_tab[linepos_der_index] = // last der value + (linepos_der_tab[i] + e - speed_left_e_old) * K_ATT_LINEPOS_DER; + + speed_linepos_der = 0; + for(i=0 ; i < N_LINEPOS_DER_TAB ; i++) // calculating sum + speed_linepos_der += linepos_der_tab[i]; + + /* Integral update (reusing left channel variables). */ + 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 */ + + dsp_mul_i16f88 (speed_linepos_der, speed_kd); /* 15b = 11b * 4.8b */ + + /* Save result. */ + speed_left_e_old = e; + if(pwm > 0) + { + pwm_left = linefol_max_pwm * (PWM_MAX - pwm); + pwm_right = linefol_max_pwm * (PWM_MAX); + } + else + { + pwm_left = linefol_max_pwm * PWM_MAX; + pwm_right = linefol_max_pwm * (PWM_MAX - pwm); + } +} + /** Compute new pwm value for left motor. */ static inline void -- cgit v1.2.3