summaryrefslogtreecommitdiff
path: root/n/line-follower
diff options
context:
space:
mode:
authorprot2004-12-19 18:49:17 +0000
committerprot2004-12-19 18:49:17 +0000
commit006bbbb9bc0909e8d68b8e40289e180a81cb806b (patch)
treef07c4ecda2e5fc06a39f3a09db588bbeb771fc89 /n/line-follower
parent10713d68a2db1c61516a1c823cf855ea858ff497 (diff)
Added support for plotting p, i, and d components of the calculated pid
Diffstat (limited to 'n/line-follower')
-rw-r--r--n/line-follower/src/main.c21
-rw-r--r--n/line-follower/src/speed.c10
2 files changed, 27 insertions, 4 deletions
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)