From 82eca8372fdd8c56a136b09b534e4369ce4a2486 Mon Sep 17 00:00:00 2001 From: prot Date: Sun, 12 Dec 2004 00:34:30 +0000 Subject: Added protocol for mode 3 and 4 Renaming variables -> Quite good for test ! --- n/line-follower/src/linesensor.c | 9 +++++--- n/line-follower/src/main.c | 42 ++++++++++++++++++++++++++++++----- n/line-follower/src/speed.c | 47 ++++++++++++++++++++-------------------- 3 files changed, 66 insertions(+), 32 deletions(-) (limited to 'n/line-follower') diff --git a/n/line-follower/src/linesensor.c b/n/line-follower/src/linesensor.c index 8daf00f..c43165a 100644 --- a/n/line-follower/src/linesensor.c +++ b/n/line-follower/src/linesensor.c @@ -1,3 +1,5 @@ +#ifndef linesensor_c +#define linesensor_c /* linesensor.c */ /* linefol - Line follower robot on a ATmega128. {{{ * @@ -25,7 +27,7 @@ /** The line position between -8 and +8. */ static int8_t linepos; -/** The number of bits seeing the line. */ +/** The number of bits seeing a line ( ~= width of the line). */ static int8_t lineActiveBits; /* +AutoDec */ @@ -41,7 +43,7 @@ linesensor_poll (void); /** Poll linesensor and returns the state on the RS232 output. * For test, calibration and verification. */ static inline void -linesensor_poll_human (void); +linesensor_print (void); /* -AutoDec */ @@ -86,7 +88,7 @@ linesensor_poll (void) /** Poll linesensor and returns the state on the RS232 output. * For test, calibration and verification. */ static inline void -linesensor_poll_human (void) +linesensor_print (void) { uint8_t c, a, i; c = PINC; @@ -112,3 +114,4 @@ linesensor_poll_human (void) rs232_putc ('\r'); } +#endif // linesensor_c diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c index a8aceb2..1c89604 100644 --- a/n/line-follower/src/main.c +++ b/n/line-follower/src/main.c @@ -34,12 +34,14 @@ #include "counter.c" #include "speed.c" #include "postrack.c" +#include "linesensor.c" /** Motor mode : * 0 - pwm setup; * 1 - speed control; * 2 - position control; - * 3 - linefol mode. */ + * 3 - linefol mode; + * 4 - linefol calibration mode. */ int8_t motor_mode; /** Main loop counter. */ @@ -108,15 +110,22 @@ main_loop (void) postrack_update (); motor_timer_2 = timer_read (); /* Speed control. */ - if (motor_mode == 3) - { - speed_compute_linefol_pwm (); - } - else if (motor_mode >= 1) + switch (motor_mode) { + case 1: + case 2: speed_update (); speed_compute_left_pwm (); speed_compute_right_pwm (); + case 3: + linesensor_poll (); + speed_compute_linefol_pwm (); + case 4: + delay_ms (5L); + linesensor_print (); + default: + motor_mode = motor_mode; // j'ai pas trouvé comment faire + // autrement... } motor_timer_1 = timer_read (); /* Pwm setup. */ @@ -202,6 +211,24 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) speed_left_aim = argv[0]; speed_right_aim = argv[1]; break; + case c ('l', 0): + motor_mode = 3; + linesensor_init (); + break; + case c ('l', 1): + motor_mode = 3; + linesensor_init (); + linefol_max_pwm = argv[0]; + break; + case c ('l', 5): + motor_mode = 3; + linesensor_init (); + linefol_max_pwm = argv[0]; + speed_kp = argv[1]; + speed_ki = argv[2]; + speed_kd = argv[3]; + K_ATT_LINEPOS_DER = argv[4]; + break; case c ('a', 1): speed_acc_cpt = speed_acc = argv[0]; break; @@ -211,6 +238,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) case c ('i', 2): speed_ki = argv[0] << 8 | argv[1]; break; + case c ('d', 2): + speed_kd = argv[0] << 8 | argv[1]; + break; case c ('f', 2): postrack_set_footing (argv[0] << 8 | argv[1]); break; diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c index ed883a5..99c7023 100644 --- a/n/line-follower/src/speed.c +++ b/n/line-follower/src/speed.c @@ -23,17 +23,18 @@ * * }}} */ +#include "linesensor.c" -/** Maximum value the pwm can reach (defined by the avr configuration) */ +/** 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) */ +/** 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 */ +/** Attenuation for filtering the derivate term (f88). */ +uint16_t K_ATT_LINEPOS_DER = 0x00FE; // ~= 95% +/** 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; -/** Attenuation for filtering the derivate term */ -#define K_ATT_LINEPOS_DER 0.95 /** Actual speed. */ int8_t speed_left, speed_right; @@ -44,11 +45,12 @@ uint8_t speed_acc = 8; /** Acceleration counter, speed gets updated when it reachs 0. */ uint8_t speed_acc_cpt; /** Integral term. */ -int16_t speed_left_int, speed_right_int; +int16_t speed_left_int, speed_right_int, linepos_int; /** Integral max value. */ int16_t speed_int_max = 1024; +int16_t linepos_int_max = 1024; /** Last error value. */ -int16_t speed_left_e_old, speed_right_e_old; +int16_t speed_left_e_old, speed_right_e_old, e_old; /** P coeficients. 5.8 fixed point format. */ uint16_t speed_kp = 2 * 255; /** I coeficients. 4.8 fixed point format. */ @@ -56,7 +58,6 @@ uint16_t speed_ki = 1 * 255; /** D coeficient. 4.8 fixed point format. */ uint16_t speed_kd = 1 * 255; -#include "linesensor.c" /* +AutoDec */ @@ -124,40 +125,40 @@ speed_update (void) static inline void speed_compute_linefol_pwm (void) { - int16_t e,speed_linepos_der; + int16_t e, linepos_der; uint8_t i; int16_t pwm; // reusing left channel variables - linesensor_poll (void); e = linepos; /* 10b = 8b + 9b */ /* Derivative update. */ i=linepos_der_index; // old index - linepos_der_index= // updating index by + linepos_der_index= // updating new index (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; + linepos_der_tab[linepos_der_index] = // new der value + dsp_mul_i16f88 ((linepos_der_tab[i] + e - e_old), + K_ATT_LINEPOS_DER); - speed_linepos_der = 0; + linepos_der = 0; for(i=0 ; i < N_LINEPOS_DER_TAB ; i++) // calculating sum - speed_linepos_der += linepos_der_tab[i]; + 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; + linepos_int += e; /* 12b = 11b + 10b */ + if (linepos_int > linepos_int_max) /* 11b */ + linepos_int = linepos_int_max; + else if (linepos_int < -linepos_int_max) + linepos_int = -linepos_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 */ + + dsp_mul_i16f88 (linepos_int, speed_ki) /* 15b = 11b * 4.8b */ + + dsp_mul_i16f88 (linepos_der, speed_kd); /* 15b = 11b * 4.8b */ /* Save result. */ - speed_left_e_old = e; + e_old = e; if(pwm > 0) { pwm_left = linefol_max_pwm * (PWM_MAX - pwm); -- cgit v1.2.3