Dependents: nhk_2018_undercarry_test04 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
Diff: PID.cpp
- Revision:
- 0:d8b5fa590ca2
- Child:
- 1:90e1008a1bf9
diff -r 000000000000 -r d8b5fa590ca2 PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Tue Jan 16 14:27:31 2018 +0000 @@ -0,0 +1,29 @@ +#include "PID.h" +#include "mbed.h" + +PID::PID(double kp, double ki, double kd, double setpoint){ + KP = kp; + KI = ki; + KD = kd; + Setpoint = setpoint; + init(); +} + +void PID::init(){ + timer.reset(); + timer.start(); + preTime = 0; + P = 0; + I = 0; + D = 0; +} + +double PID::compute(double input){ + dt = (timer.read() - preTime) / 10000; + preTime = timer.read(); + P = Setpoint - input; + I += P * dt; + D = (P - preP) / dt; + preP = P; + return (KP * P + KI * I + KD * D); +}