From 006bbbb9bc0909e8d68b8e40289e180a81cb806b Mon Sep 17 00:00:00 2001 From: prot Date: Sun, 19 Dec 2004 18:49:17 +0000 Subject: Added support for plotting p, i, and d components of the calculated pid --- n/line-follower/src/main.c | 21 ++++++++++++++++++--- n/line-follower/src/speed.c | 10 +++++++++- 2 files changed, 27 insertions(+), 4 deletions(-) (limited to 'n/line-follower') diff --git a/n/line-follower/src/main.c b/n/line-follower/src/main.c index 96c4629..1b7e857 100644 --- a/n/line-follower/src/main.c +++ b/n/line-follower/src/main.c @@ -53,6 +53,10 @@ uint8_t motor_stat_speed, motor_stat_speed_cpt; /** Statistics about pwm values. */ 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; @@ -123,9 +127,6 @@ main_loop (void) 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. */ @@ -141,6 +142,17 @@ main_loop (void) speed_right_int >> 8, speed_right_int); motor_stat_speed_cpt = motor_stat_speed; } + // displays stats concerning pid components + if (motor_stat_pid && !--motor_stat_pid_cpt) + { + proto_send4 ('C', + pid_pid >> 8, pid_pid, + pid_p >> 8, pid_p); + proto_send4 ('.', + pid_i >> 8, pid_i, + pid_d >> 8, pid_d); + motor_stat_pid_cpt = motor_stat_pid; + } if (motor_stat_pwm && !--motor_stat_pwm_cpt) { proto_send4 ('W', @@ -248,6 +260,9 @@ 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): + motor_stat_pid_cpt = motor_stat_pid = argv[0]; + break; case c ('c', 1): motor_stat_counter_cpt = motor_stat_counter = argv[0]; break; diff --git a/n/line-follower/src/speed.c b/n/line-follower/src/speed.c index 25ec08c..cafd446 100644 --- a/n/line-follower/src/speed.c +++ b/n/line-follower/src/speed.c @@ -25,6 +25,9 @@ #include "linesensor.c" +/** 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). */ @@ -179,12 +182,17 @@ speed_compute_linefol_pwm (void) 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 (linepos_int, speed_ki) /* 15b = 11b * 4.8b */ + dsp_mul_i16f88 (linepos_der, speed_kd); /* 15b = 11b * 4.8b */ + pid_p = e; + pid_i = linepos_int; + pid_d = linepos_der; + pid_pid = pwm; + /* Save result. */ e_old = e; if(pwm > 0) -- cgit v1.2.3