62 أسطر
1.3 KiB
C++
62 أسطر
1.3 KiB
C++
#include <float.h>
|
|
#include "SimplePID.h"
|
|
|
|
SimplePID::SimplePID(float Kp, float Ki, float Kd) {
|
|
this->Kp = Kp;
|
|
this->Ki = Ki;
|
|
this->Kd = Kd;
|
|
|
|
minOutput = -FLT_MAX;
|
|
maxOutput = FLT_MAX;
|
|
|
|
sumError = 0.0;
|
|
lastActual = 0.0;
|
|
}
|
|
|
|
void SimplePID::setConstants(float Kp, float Ki, float Kd) {
|
|
this->Kp = Kp;
|
|
this->Ki = Ki;
|
|
this->Kd = Kd;
|
|
}
|
|
|
|
void SimplePID::setOutputRange(float minOutput, float maxOutput) {
|
|
this->minOutput = minOutput;
|
|
this->maxOutput = maxOutput;
|
|
}
|
|
|
|
void SimplePID::setSetPoint(float setPoint) {
|
|
this->setPoint = setPoint;
|
|
sumError = 0.0;
|
|
}
|
|
|
|
float SimplePID::getCumulativeError() {
|
|
return sumError;
|
|
}
|
|
|
|
void SimplePID::clearCumulativeError() {
|
|
sumError = 0.0;
|
|
}
|
|
|
|
float SimplePID::getControlValue(float actual, float dt) {
|
|
float error = setPoint - actual;
|
|
|
|
float newSum = sumError + error*dt;
|
|
|
|
// The derivative is calculated by assuming the two setpoints are
|
|
// equal. This works better when changing the setpoint because the
|
|
// derivative error does not suddenly increase.
|
|
float dErrorDt = (lastActual - actual) / dt;
|
|
lastActual = actual;
|
|
|
|
float output = Kp*error + Ki*sumError + Kd*dErrorDt;
|
|
|
|
if (output >= maxOutput) {
|
|
return maxOutput;
|
|
} else if (output <= minOutput) {
|
|
return minOutput;
|
|
} else {
|
|
sumError = newSum;
|
|
return output;
|
|
}
|
|
}
|