forked from br3ttb/Arduino-PID-Library
-
Notifications
You must be signed in to change notification settings - Fork 48
/
QuickPID.cpp
263 lines (235 loc) · 10.3 KB
/
QuickPID.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
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
/**********************************************************************************
QuickPID Library for Arduino - Version 3.1.2
by dlloydev https://github.com/Dlloydev/QuickPID
Based on the Arduino PID_v1 Library. Licensed under the MIT License.
**********************************************************************************/
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "QuickPID.h"
/* Constructor ********************************************************************
The parameters specified here are those for for which we can't set up
reliable defaults, so we need to have the user set them.
**********************************************************************************/
QuickPID::QuickPID(float* Input, float* Output, float* Setpoint,
float Kp = 0, float Ki = 0, float Kd = 0,
pMode pMode = pMode::pOnError,
dMode dMode = dMode::dOnMeas,
iAwMode iAwMode = iAwMode::iAwCondition,
Action Action = Action::direct) {
myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
mode = Control::manual;
QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit
sampleTimeUs = 100000; // 0.1 sec default
QuickPID::SetControllerDirection(Action);
QuickPID::SetTunings(Kp, Ki, Kd, pMode, dMode, iAwMode);
lastTime = micros() - sampleTimeUs;
}
/* Constructor *********************************************************************
To allow using pOnError, dOnMeas and iAwCondition without explicitly saying so.
**********************************************************************************/
QuickPID::QuickPID(float* Input, float* Output, float* Setpoint,
float Kp, float Ki, float Kd, Action Action)
: QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd,
pmode = pMode::pOnError,
dmode = dMode::dOnMeas,
iawmode = iAwMode::iAwCondition,
action = Action) {
}
/* Constructor *********************************************************************
Simplified constructor which uses defaults for remaining parameters.
**********************************************************************************/
QuickPID::QuickPID(float* Input, float* Output, float* Setpoint)
: QuickPID::QuickPID(Input, Output, Setpoint, dispKp, dispKi, dispKd,
pmode = pMode::pOnError,
dmode = dMode::dOnMeas,
iawmode = iAwMode::iAwCondition,
action = Action::direct) {
}
/* Compute() ***********************************************************************
This function should be called every time "void loop()" executes. The function
will decide whether a new PID Output needs to be computed. Returns true
when the output is computed, false when nothing has been done.
**********************************************************************************/
bool QuickPID::Compute() {
if (mode == Control::manual) return false;
uint32_t now = micros();
uint32_t timeChange = (now - lastTime);
if (mode == Control::timer || timeChange >= sampleTimeUs) {
float input = *myInput;
float dInput = input - lastInput;
if (action == Action::reverse) dInput = -dInput;
error = *mySetpoint - input;
if (action == Action::reverse) error = -error;
float dError = error - lastError;
float peTerm = kp * error;
float pmTerm = kp * dInput;
if (pmode == pMode::pOnError) pmTerm = 0;
else if (pmode == pMode::pOnMeas) peTerm = 0;
else { //pOnErrorMeas
peTerm *= 0.5f;
pmTerm *= 0.5f;
}
pTerm = peTerm - pmTerm; // used by GetDterm()
iTerm = ki * error;
if (dmode == dMode::dOnError) dTerm = kd * dError;
else dTerm = -kd * dInput; // dOnMeas
//condition anti-windup (default)
if (iawmode == iAwMode::iAwCondition) {
bool aw = false;
float iTermOut = (peTerm - pmTerm) + ki * (iTerm + error);
if (iTermOut > outMax && dError > 0) aw = true;
else if (iTermOut < outMin && dError < 0) aw = true;
if (aw && ki) iTerm = constrain(iTermOut, -outMax, outMax);
}
// by default, compute output as per PID_v1
outputSum += iTerm; // include integral amount
if (iawmode == iAwMode::iAwOff) outputSum -= pmTerm; // include pmTerm (no anti-windup)
else outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp
*myOutput = constrain(outputSum + peTerm + dTerm, outMin, outMax); // include dTerm, clamp and drive output
lastError = error;
lastInput = input;
lastTime = now;
return true;
}
else return false;
}
/* SetTunings(....)************************************************************
This function allows the controller's dynamic performance to be adjusted.
it's called automatically from the constructor, but tunings can also
be adjusted on the fly during normal operation.
******************************************************************************/
void QuickPID::SetTunings(float Kp, float Ki, float Kd,
pMode pMode = pMode::pOnError,
dMode dMode = dMode::dOnMeas,
iAwMode iAwMode = iAwMode::iAwCondition) {
if (Kp < 0 || Ki < 0 || Kd < 0) return;
if (Ki == 0) outputSum = 0;
pmode = pMode; dmode = dMode; iawmode = iAwMode;
dispKp = Kp; dispKi = Ki; dispKd = Kd;
float SampleTimeSec = (float)sampleTimeUs / 1000000;
kp = Kp;
ki = Ki * SampleTimeSec;
kd = Kd / SampleTimeSec;
}
/* SetTunings(...)************************************************************
Set Tunings using the last remembered pMode, dMode and iAwMode settings.
******************************************************************************/
void QuickPID::SetTunings(float Kp, float Ki, float Kd) {
SetTunings(Kp, Ki, Kd, pmode, dmode, iawmode);
}
/* SetSampleTime(.)***********************************************************
Sets the period, in microseconds, at which the calculation is performed.
******************************************************************************/
void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) {
if (NewSampleTimeUs > 0) {
float ratio = (float)NewSampleTimeUs / (float)sampleTimeUs;
ki *= ratio;
kd /= ratio;
sampleTimeUs = NewSampleTimeUs;
}
}
/* SetOutputLimits(..)********************************************************
The PID controller is designed to vary its output within a given range.
By default this range is 0-255, the Arduino PWM range.
******************************************************************************/
void QuickPID::SetOutputLimits(float Min, float Max) {
if (Min >= Max) return;
outMin = Min;
outMax = Max;
if (mode != Control::manual) {
*myOutput = constrain(*myOutput, outMin, outMax);
outputSum = constrain(outputSum, outMin, outMax);
}
}
/* SetMode(.)*****************************************************************
Sets the controller mode to manual (0), automatic (1) or timer (2)
when the transition from manual to automatic or timer occurs, the
controller is automatically initialized.
******************************************************************************/
void QuickPID::SetMode(Control Mode) {
if (mode == Control::manual && Mode != Control::manual) { // just went from manual to automatic or timer
QuickPID::Initialize();
}
mode = Mode;
}
/* Initialize()****************************************************************
Does all the things that need to happen to ensure a bumpless transfer
from manual to automatic mode.
******************************************************************************/
void QuickPID::Initialize() {
outputSum = *myOutput;
lastInput = *myInput;
outputSum = constrain(outputSum, outMin, outMax);
}
/* SetControllerDirection(.)**************************************************
The PID will either be connected to a direct acting process (+Output leads
to +Input) or a reverse acting process(+Output leads to -Input).
******************************************************************************/
void QuickPID::SetControllerDirection(Action Action) {
action = Action;
}
/* SetProportionalMode(.)*****************************************************
Sets the computation method for the proportional term, to compute based
either on error (default), on measurement, or the average of both.
******************************************************************************/
void QuickPID::SetProportionalMode(pMode pMode) {
pmode = pMode;
}
/* SetDerivativeMode(.)*******************************************************
Sets the computation method for the derivative term, to compute based
either on error or on measurement (default).
******************************************************************************/
void QuickPID::SetDerivativeMode(dMode dMode) {
dmode = dMode;
}
/* SetAntiWindupMode(.)*******************************************************
Sets the integral anti-windup mode to one of iAwClamp, which clamps
the output after adding integral and proportional (on measurement) terms,
or iAwCondition (default), which provides some integral correction, prevents
deep saturation and reduces overshoot.
Option iAwOff disables anti-windup altogether.
******************************************************************************/
void QuickPID::SetAntiWindupMode(iAwMode iAwMode) {
iawmode = iAwMode;
}
/* Status Functions************************************************************
These functions query the internal state of the PID.
******************************************************************************/
float QuickPID::GetKp() {
return dispKp;
}
float QuickPID::GetKi() {
return dispKi;
}
float QuickPID::GetKd() {
return dispKd;
}
float QuickPID::GetPterm() {
return pTerm;
}
float QuickPID::GetIterm() {
return iTerm;
}
float QuickPID::GetDterm() {
return dTerm;
}
uint8_t QuickPID::GetMode() {
return static_cast<uint8_t>(mode);
}
uint8_t QuickPID::GetDirection() {
return static_cast<uint8_t>(action);
}
uint8_t QuickPID::GetPmode() {
return static_cast<uint8_t>(pmode);
}
uint8_t QuickPID::GetDmode() {
return static_cast<uint8_t>(dmode);
}
uint8_t QuickPID::GetAwMode() {
return static_cast<uint8_t>(iawmode);
}