summaryrefslogtreecommitdiff
path: root/n/line-follower/src/speed.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/line-follower/src/speed.c')
-rw-r--r--n/line-follower/src/speed.c10
1 files changed, 9 insertions, 1 deletions
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)