![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
baseline build
Dependencies: FastPWM mbed-os mbed
Diff: PID.cpp
- Revision:
- 0:8a420ac6394e
diff -r 000000000000 -r 8a420ac6394e PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Mon Jun 19 15:55:51 2017 +0000 @@ -0,0 +1,72 @@ + +#include "PID.h" + +const int SCALING_FACTOR = 10000; + +PID::PID(void) +{ + minOutput = 0; + maxOutput = 0; + kp = 0; + ki = 0; + kd = 0; + errorIntegral = 0; + lastError = 0; + msPeriod = 1; + target = 0; + currentOutput = 0; +} + + +void PID::Reset(void) +{ + errorIntegral = 0; + lastError = 0; + currentOutput = 0; +} + + +void PID::SetOutputRange(double minOutput, double maxOutput) +{ + this->minOutput = minOutput; + this->maxOutput = maxOutput; +} + + +void PID::SetPID(double kp, double ki, double kd) +{ + this->kp = (kp/SCALING_FACTOR); + this->ki = (ki/SCALING_FACTOR); + this->kd = (kd/SCALING_FACTOR); +} + + +void PID::AddSample(double sampleValue, double control) +{ + pidSampleValue = sampleValue; + // calculate error + double error = target - sampleValue; + + // update our integral; the I & D gain are assumed to use seconds as their units + errorIntegral += error * msPeriod / 1000; + + // calculate our differential + double errorDifferential = (error - lastError) / (msPeriod / 1000); + lastError = error; + + // that gives us what we need to calculate an output + double output = control+( + kp * error + + ki * errorIntegral + + kd * errorDifferential) + ; + if (output < minOutput) + output = minOutput; + if (output > maxOutput) + output = maxOutput; + + currentOutput = output; +} + + +