diff options
Diffstat (limited to 'zumo/pid.cpp')
| -rw-r--r-- | zumo/pid.cpp | 27 |
1 files changed, 23 insertions, 4 deletions
diff --git a/zumo/pid.cpp b/zumo/pid.cpp index 9bef08f..fabe1da 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -1,9 +1,14 @@ #include "pid.h" PID::PID(float P, float I, float D) { - A0 = P + I*dt + D/dt; - A1 = -P - 2*D/dt; - A2 = D/dt; + A0 = P + I * dt; + A1 = -P; + A0d = D / dt; + A1d = -2 * D / dt; + A2d = D / dt; + tau = D / (P * N); + alpha = dt / (2 * tau); + reset(0.0); } // https://en.wikipedia.org/wiki/PID_controller#Pseudocode @@ -11,7 +16,17 @@ float PID::iter(float target) { error[2] = error[1]; error[1] = error[0]; error[0] = target - output; - output = output + A0 * error[0] + A1 * error[1] + A2 * error[2]; + + output = output + A0 * error[0] + A1 * error[1]; + + d1 = d0; + d0 = A0d * error[0] + A1d * error[1] + A2d * error[2]; + fd1 = fd0; + fd0 = ((alpha) / (alpha + 1)) * (d0 + d1) - ((alpha - 1) / (alpha + 1)) * fd1; + output = output + fd0; + + if (output < -1) output = -1; + if (output > 1) output = 1; return output; } @@ -19,6 +34,10 @@ void PID::reset(float value) { error[0] = 0.0; error[1] = 0.0; error[2] = 0.0; + d0 = 0; + d1 = 0; + fd0 = 0; + fd1 = 0; output = value; } |