-
Notifications
You must be signed in to change notification settings - Fork 3
/
SimplePID.cpp
61 lines (48 loc) · 1.29 KB
/
SimplePID.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
#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;
}
}