From 792e00386f06f5d71ae4019e4d07ee335290608e Mon Sep 17 00:00:00 2001 From: prot Date: Mon, 20 Dec 2004 15:49:47 +0000 Subject: Added : - specialized coefs for linefol - protocol for changing these coefs ('I') - protocol for pid components stats ('C') --- n/line-follower/src/avrconfig.h | 2 +- n/line-follower/src/main.c | 25 ++++++++++++++++--------- n/line-follower/src/pwm.c | 8 ++++---- n/line-follower/src/speed.c | 12 ++++-------- 4 files changed, 25 insertions(+), 22 deletions(-) diff --git a/n/line-follower/src/avrconfig.h b/n/line-follower/src/avrconfig.h index bb19576..be26c24 100644 --- a/n/line-follower/src/avrconfig.h +++ b/n/line-follower/src/avrconfig.h @@ -55,7 +55,7 @@ /* proto - Protocol module. */ /** Maximum argument number. */ -#define AC_PROTO_MAX_ARGS 4 +#define AC_PROTO_MAX_ARGS 6 /** Protocol arguments type. */ #define AC_PROTO_ARG_TYPE uint8_t diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c index 3e7d84e..91b5a8b 100644 --- a/n/line-follower/src/main.c +++ b/n/line-follower/src/main.c @@ -55,7 +55,6 @@ uint8_t motor_stat_pwm, motor_stat_pwm_cpt; /** Statistics about pid components. */ uint8_t motor_stat_pid, motor_stat_pid_cpt; -int16_t pid_pid,pid_p,pid_i,pid_d /** Report of timer. */ uint8_t motor_stat_timer, motor_stat_timer_cpt; @@ -241,18 +240,14 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) linesensor_reset (); linefol_max_pwm = argv[0]; break; - case c ('l', 5): + case c ('l', 3):// set linefol general coefs : "!lmmKKKK" motor_mode = 3; linesensor_reset (); linefol_max_pwm = argv[0]; - speed_kp = argv[1]; - speed_ki = argv[2]; - speed_kd = argv[3]; - K_ATT_LINEPOS_DER = argv[4]; + K_ATT_LINEPOS_DER = argv[1] << 8 | argv[2]; break; case c ('a', 1): speed_acc_cpt = speed_acc = argv[0]; - break; case c ('p', 2): speed_kp = argv[0] << 8 | argv[1]; break; @@ -261,6 +256,18 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) break; case c ('d', 2): speed_kd = argv[0] << 8 | argv[1]; + break;break; + case c ('I', 6): // set lInefol pid coefs : "!Ippppiiiidddd" + speed_kp = argv[0] << 8 | argv[1]; + speed_ki = argv[2] << 8 | argv[3]; + speed_kd = argv[4] << 8 | argv[5]; + break; + case c ('I', 4): // set lInefol pid coefs : "!Ippppiiiidddd" + speed_kp = argv[0] << 8 | argv[1]; + speed_ki = argv[2] << 8 | argv[3]; + break; + case c ('I', 2): // set lInefol pid coefs : "!Ippppiiiidddd" + speed_kp = argv[0] << 8 | argv[1]; break; case c ('f', 2): postrack_set_footing (argv[0] << 8 | argv[1]); @@ -269,7 +276,7 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) motor_stat_speed_cpt = motor_stat_speed = argv[0]; motor_stat_pwm_cpt = motor_stat_pwm = argv[0]; break; - case c ('C', 1): + case c ('C', 1): // display components of PID motor_stat_pid_cpt = motor_stat_pid = argv[0]; break; case c ('c', 1): @@ -281,7 +288,7 @@ 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): + case c ('P', 1): // switch PWM frequency mode pwm_init (argv[0]); break; default: diff --git a/n/line-follower/src/pwm.c b/n/line-follower/src/pwm.c index 9c5d15e..15da89d 100644 --- a/n/line-follower/src/pwm.c +++ b/n/line-follower/src/pwm.c @@ -51,7 +51,7 @@ pwm_init (uint8_t n) Fpwm = f_IO / (2 * prescaler * TOP) = 28912 Hz. */ switch (n) { - case 0: + case 0:// f = f0 = 28912 Hz, PWM = 8bits default: TCCR1A = regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, @@ -59,21 +59,21 @@ pwm_init (uint8_t n) TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, 0, 0, 0, 0, 0, 0, 0, 1); break; - case 1: + case 1:// f = f0 / 2 = 14456 Hz, PWM = 9bits 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: + case 2:// f = f0 / 4 = 7228 Hz, PWM = 10 bits 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: + case 3:// f = f0 / 8 = 3614 Hz, PWM = 8 bits TCCR1A = regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, 0, 0, 1, 0, 1, 0, 0, 1); diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c index 8eaa9ba..ea31286 100644 --- a/n/line-follower/src/speed.c +++ b/n/line-follower/src/speed.c @@ -29,16 +29,16 @@ extern uint8_t motor_mode; /** Statistics about pid components. */ -extern int16_t pid_pid,pid_p,pid_i,pid_d +int16_t pid_pid,pid_p,pid_i,pid_d; /** Coefs for linefol mode. */ -extern uint16_t line_kp,line_ki,line_kd +uint16_t line_kp,line_ki,line_kd; /** 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). */ -int8_t linefol_max_pwm = 80; -/** Attenuation for filtering the derivate term (f88). */ +uint8_t linefol_max_pwm = 80; +/** Attenuation for filtering the derivate term. */ uint16_t K_ATT_LINEPOS_DER = 0x00FE; // ~= 95% /** Table and index for calculating the derivate term. */ #define N_LINEPOS_DER_TAB 8 // max : 255 (8bit-addressing) @@ -207,15 +207,11 @@ speed_compute_linefol_pwm (void) { 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 -- cgit v1.2.3