From 8b9bbc5af08793dcf9869703a81381ca1b97884c Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 19 Dec 2004 20:27:11 +0000 Subject: asservissement vitesse tests chez nico --- n/line-follower/src/linesensor.c | 16 ++++++++----- n/line-follower/src/main.c | 16 +++++++++++-- n/line-follower/src/pwm.c | 41 ++++++++++++++++++++++++++------ n/line-follower/src/speed.c | 50 +++++++++++++++++++++++++++++++--------- 4 files changed, 97 insertions(+), 26 deletions(-) (limited to 'n/line-follower') diff --git a/n/line-follower/src/linesensor.c b/n/line-follower/src/linesensor.c index e425e66..30d70fd 100644 --- a/n/line-follower/src/linesensor.c +++ b/n/line-follower/src/linesensor.c @@ -3,7 +3,7 @@ /* linesensor.c */ /* linefol - Line follower robot on a ATmega128. {{{ * - * Copyright (C) 2004 Nicolas Schodet + * Copyright (C) 2004 Pierre Prot * * Robot APB Team/Efrei 2005. * Web: http://assos.efrei.fr/robot/ @@ -48,8 +48,8 @@ static inline void linesensor_poll (void) { uint8_t c, a, i, sum; - c = PINC; - a = PINA; + c = PINC | 0x01; + a = PINA | 0x80; sum=0; lineActiveBits = 0; @@ -83,9 +83,11 @@ static inline void linesensor_print (void) { uint8_t c, a, i; - c = PINC; - a = PINA; + c = PINC | 0x01; + a = PINA | 0x80; + linesensor_poll (); + for (i = 0; i < 8; i++) { if (!(c & 1)) @@ -103,7 +105,9 @@ linesensor_print (void) rs232_putc ('.'); a >>= 1; } - rs232_putc ('\r'); + rs232_putc (' '); + + proto_send1('P',(linepos + 15)); } #endif // linesensor_c diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c index 1b7e857..3e7d84e 100644 --- a/n/line-follower/src/main.c +++ b/n/line-follower/src/main.c @@ -42,7 +42,7 @@ * 2 - position control; * 3 - linefol mode; * 4 - linefol calibration mode. */ -int8_t motor_mode; +uint8_t motor_mode; /** Main loop counter. */ uint8_t motor_loop_cpt; @@ -87,7 +87,7 @@ int main (void) { DDRD = 0x60; - pwm_init (); + pwm_init (0); timer_init (); counter_init (); speed_init (); @@ -121,12 +121,17 @@ main_loop (void) speed_update (); speed_compute_left_pwm (); speed_compute_right_pwm (); + break; case 3: linesensor_poll (); speed_compute_linefol_pwm (); + speed_compute_left_pwm (); + speed_compute_right_pwm (); + break; case 4: delay_ms (5L); linesensor_print (); + break; } motor_timer_1 = timer_read (); /* Pwm setup. */ @@ -223,6 +228,10 @@ 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 = 4; + linesensor_reset (); + break; case c ('l', 0): motor_mode = 3; linesensor_reset (); @@ -272,6 +281,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) case c ('T', 1): motor_stat_postrack_cpt = motor_stat_postrack = argv[0]; break; + case c ('P', 1): + pwm_init (argv[0]); + break; default: proto_send0 ('e'); return; diff --git a/n/line-follower/src/pwm.c b/n/line-follower/src/pwm.c index 883201f..9c5d15e 100644 --- a/n/line-follower/src/pwm.c +++ b/n/line-follower/src/pwm.c @@ -30,7 +30,7 @@ int16_t pwm_left, pwm_right; /** Initialise PWM generator. */ static inline void -pwm_init (void); +pwm_init (uint8_t n); /** Preprocess a PWM value before sending it to hardware. */ static inline uint8_t @@ -44,16 +44,43 @@ pwm_update (void); /** Initialise PWM generator. */ static inline void -pwm_init (void) +pwm_init (uint8_t n) { /* Phase correct PWM, TOP = 0xff, OC1B & OC1C with positive logic. f_IO without prescaler. 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); - TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, - 0, 0, 0, 0, 0, 0, 0, 1); + switch (n) + { + case 0: + default: + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 0, 0, 1, 0, 1, 0, 0, 1); + TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, + 0, 0, 0, 0, 0, 0, 0, 1); + break; + case 1: + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 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); + break; + case 2: + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 0, 0, 1, 0, 1, 0, 1, 1); + TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, + 0, 0, 0, 0, 0, 0, 0, 1); + break; + case 3: + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 0, 0, 1, 0, 1, 0, 0, 1); + TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, + 0, 0, 0, 0, 0, 0, 1, 0); + break; + } /* Enable pwm and direction outputs in DDRB. */ DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2); } diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c index cafd446..e23c52d 100644 --- a/n/line-follower/src/speed.c +++ b/n/line-follower/src/speed.c @@ -25,13 +25,16 @@ #include "linesensor.c" +// C'est porc : +extern uint8_t motor_mode; + /** Statistics about pid components. */ extern int16_t pid_pid,pid_p,pid_i,pid_d /** 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; +int8_t linefol_max_pwm = 80; /** Attenuation for filtering the derivate term (f88). */ uint16_t K_ATT_LINEPOS_DER = 0x00FE; // ~= 95% /** Table and index for calculating the derivate term. */ @@ -39,7 +42,7 @@ uint16_t K_ATT_LINEPOS_DER = 0x00FE; // ~= 95% int16_t linepos_der_tab[N_LINEPOS_DER_TAB]; uint8_t linepos_der_index; -/** Actual speed. */ +/** Current speed. */ int8_t speed_left, speed_right; /** Wanted speed. */ int8_t speed_left_aim, speed_right_aim; @@ -64,10 +67,6 @@ uint16_t speed_kd = 1 * 255; /* +AutoDec */ -/** Update speeds according to the wanted speeds and the acceleration. */ -static inline void -speed_update (void); - /** Initialise speed parameters. */ static inline void speed_init (void); @@ -84,6 +83,9 @@ speed_update (void); static inline void speed_compute_linefol_pwm (void); +static inline int8_t +linefol_saturate (int16_t input); + /** Compute new pwm value for left motor. */ static inline void speed_compute_left_pwm (void); @@ -152,6 +154,7 @@ 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) @@ -195,18 +198,43 @@ speed_compute_linefol_pwm (void) /* Save result. */ e_old = e; - if(pwm > 0) + if(lineActiveBits > 1) { - pwm_left = linefol_max_pwm * (PWM_MAX - pwm); - pwm_right = linefol_max_pwm * (PWM_MAX); + if(pwm > 0) + { + speed_left = linefol_saturate (((linefol_max_pwm * PWM_MAX) >> 8) - pwm); + speed_right = linefol_saturate ((linefol_max_pwm * PWM_MAX) >> 8); +// pwm_left = ((linefol_max_pwm * PWM_MAX) >> 8) - pwm; +// pwm_right = ((linefol_max_pwm * PWM_MAX) >> 8); + } + else + { + speed_left = linefol_saturate ((linefol_max_pwm * PWM_MAX) >> 8); + speed_right = linefol_saturate (((linefol_max_pwm * PWM_MAX) >> 8 )+ pwm); +// pwm_left = ((linefol_max_pwm * PWM_MAX) >> 8); +// pwm_right = ((linefol_max_pwm * PWM_MAX) >> 8 )+ pwm; + } } else { - pwm_left = linefol_max_pwm * PWM_MAX; - pwm_right = linefol_max_pwm * (PWM_MAX - pwm); + motor_mode = 2; + speed_left_aim = 0; + speed_right_aim = 0; +// pwm_left = 0; +// pwm_right = 0; } } +static inline int8_t +linefol_saturate (int16_t input) +{ + if (input > 127) + return 127; + else if (input < -127) + return -127; + else + return input; +} /** Compute new pwm value for left motor. */ static inline void -- cgit v1.2.3