Dependents: nhk_2018_undercarry_test04 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
PID.cpp
- Committer:
- kenken0721
- Date:
- 2018-01-16
- Revision:
- 0:d8b5fa590ca2
- Child:
- 1:90e1008a1bf9
File content as of revision 0:d8b5fa590ca2:
#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); }