FRDM-K64F Code Share / PID

This is a copy of the Reference Standard PID controller ala controlguru.com

Dependents:   PIDHeater Printer PIDHeater82 UltiSaverController

Fork of PID by Arnaud Suire

PID.cpp@1:117e0c36eb22, 2016-01-25 (annotated)

Committer:
unix_guru
Date:
Mon Jan 25 22:29:38 2016 +0000
Revision:
1:117e0c36eb22
Parent:
0:d58c1b8d63d9
Child:
2:55bf0f813bb4
Old Standard PID library from ControlGuru.com

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arnaudsuire 0:d58c1b8d63d9 1 /**
arnaudsuire 0:d58c1b8d63d9 2 * Includes
arnaudsuire 0:d58c1b8d63d9 3 */
arnaudsuire 0:d58c1b8d63d9 4 #include "PID.h"
arnaudsuire 0:d58c1b8d63d9 5
arnaudsuire 0:d58c1b8d63d9 6 PID::PID(float Kc, float tauI, float tauD, float interval) {
arnaudsuire 0:d58c1b8d63d9 7
arnaudsuire 0:d58c1b8d63d9 8 usingFeedForward = false;
unix_guru 1:117e0c36eb22 9 inAuto = false;
arnaudsuire 0:d58c1b8d63d9 10
arnaudsuire 0:d58c1b8d63d9 11 //Default the limits to the full range of I/O.
arnaudsuire 0:d58c1b8d63d9 12 //Make sure to set these to more appropriate limits for your application.
arnaudsuire 0:d58c1b8d63d9 13 setInputLimits(0.0, 100.0);
arnaudsuire 0:d58c1b8d63d9 14 setOutputLimits(0.0,100.0);
arnaudsuire 0:d58c1b8d63d9 15
arnaudsuire 0:d58c1b8d63d9 16 tSample_ = interval;
arnaudsuire 0:d58c1b8d63d9 17
arnaudsuire 0:d58c1b8d63d9 18 setTunings(Kc, tauI, tauD);
arnaudsuire 0:d58c1b8d63d9 19
arnaudsuire 0:d58c1b8d63d9 20 setPoint_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 21 processVariable_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 22 prevProcessVariable_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 23 controllerOutput_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 24 prevControllerOutput_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 25
arnaudsuire 0:d58c1b8d63d9 26 accError_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 27 bias_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 28
arnaudsuire 0:d58c1b8d63d9 29 realOutput_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 30
arnaudsuire 0:d58c1b8d63d9 31 }
arnaudsuire 0:d58c1b8d63d9 32
arnaudsuire 0:d58c1b8d63d9 33 void PID::setInputLimits(float inMin, float inMax) {
arnaudsuire 0:d58c1b8d63d9 34
arnaudsuire 0:d58c1b8d63d9 35 //Make sure we haven't been given impossible values.
arnaudsuire 0:d58c1b8d63d9 36 if (inMin >= inMax) {
arnaudsuire 0:d58c1b8d63d9 37 return;
arnaudsuire 0:d58c1b8d63d9 38 }
arnaudsuire 0:d58c1b8d63d9 39
arnaudsuire 0:d58c1b8d63d9 40 //Rescale the working variables to reflect the changes.
arnaudsuire 0:d58c1b8d63d9 41 prevProcessVariable_ *= (inMax - inMin) / inSpan_;
arnaudsuire 0:d58c1b8d63d9 42 accError_ *= (inMax - inMin) / inSpan_;
arnaudsuire 0:d58c1b8d63d9 43
arnaudsuire 0:d58c1b8d63d9 44 //Make sure the working variables are within the new limits.
arnaudsuire 0:d58c1b8d63d9 45 if (prevProcessVariable_ > 1) {
arnaudsuire 0:d58c1b8d63d9 46 prevProcessVariable_ = 1;
arnaudsuire 0:d58c1b8d63d9 47 }
arnaudsuire 0:d58c1b8d63d9 48 else if (prevProcessVariable_ < 0) {
arnaudsuire 0:d58c1b8d63d9 49 prevProcessVariable_ = 0;
arnaudsuire 0:d58c1b8d63d9 50 }
arnaudsuire 0:d58c1b8d63d9 51
arnaudsuire 0:d58c1b8d63d9 52 inMin_ = inMin;
arnaudsuire 0:d58c1b8d63d9 53 inMax_ = inMax;
arnaudsuire 0:d58c1b8d63d9 54 inSpan_ = inMax - inMin;
arnaudsuire 0:d58c1b8d63d9 55
arnaudsuire 0:d58c1b8d63d9 56 }
arnaudsuire 0:d58c1b8d63d9 57
arnaudsuire 0:d58c1b8d63d9 58 void PID::setOutputLimits(float outMin, float outMax) {
arnaudsuire 0:d58c1b8d63d9 59
arnaudsuire 0:d58c1b8d63d9 60 //Make sure we haven't been given impossible values.
arnaudsuire 0:d58c1b8d63d9 61 if (outMin >= outMax) {
arnaudsuire 0:d58c1b8d63d9 62 return;
arnaudsuire 0:d58c1b8d63d9 63 }
arnaudsuire 0:d58c1b8d63d9 64
arnaudsuire 0:d58c1b8d63d9 65 //Rescale the working variables to reflect the changes.
arnaudsuire 0:d58c1b8d63d9 66 prevControllerOutput_ *= (outMax - outMin) / outSpan_;
arnaudsuire 0:d58c1b8d63d9 67
arnaudsuire 0:d58c1b8d63d9 68 //Make sure the working variables are within the new limits.
arnaudsuire 0:d58c1b8d63d9 69 if (prevControllerOutput_ > 1) {
arnaudsuire 0:d58c1b8d63d9 70 prevControllerOutput_ = 1;
arnaudsuire 0:d58c1b8d63d9 71 }
arnaudsuire 0:d58c1b8d63d9 72 else if (prevControllerOutput_ < 0) {
arnaudsuire 0:d58c1b8d63d9 73 prevControllerOutput_ = 0;
arnaudsuire 0:d58c1b8d63d9 74 }
arnaudsuire 0:d58c1b8d63d9 75
arnaudsuire 0:d58c1b8d63d9 76 outMin_ = outMin;
arnaudsuire 0:d58c1b8d63d9 77 outMax_ = outMax;
arnaudsuire 0:d58c1b8d63d9 78 outSpan_ = outMax - outMin;
arnaudsuire 0:d58c1b8d63d9 79
arnaudsuire 0:d58c1b8d63d9 80 }
arnaudsuire 0:d58c1b8d63d9 81
arnaudsuire 0:d58c1b8d63d9 82 void PID::setTunings(float Kc, float tauI, float tauD) {
arnaudsuire 0:d58c1b8d63d9 83
arnaudsuire 0:d58c1b8d63d9 84 //Verify that the tunings make sense.
arnaudsuire 0:d58c1b8d63d9 85 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
arnaudsuire 0:d58c1b8d63d9 86 return;
arnaudsuire 0:d58c1b8d63d9 87 }
arnaudsuire 0:d58c1b8d63d9 88
arnaudsuire 0:d58c1b8d63d9 89 //Store raw values to hand back to user on request.
arnaudsuire 0:d58c1b8d63d9 90 pParam_ = Kc;
arnaudsuire 0:d58c1b8d63d9 91 iParam_ = tauI;
arnaudsuire 0:d58c1b8d63d9 92 dParam_ = tauD;
arnaudsuire 0:d58c1b8d63d9 93
arnaudsuire 0:d58c1b8d63d9 94 float tempTauR;
arnaudsuire 0:d58c1b8d63d9 95
arnaudsuire 0:d58c1b8d63d9 96 if (tauI == 0.0) {
arnaudsuire 0:d58c1b8d63d9 97 tempTauR = 0.0;
arnaudsuire 0:d58c1b8d63d9 98 }
arnaudsuire 0:d58c1b8d63d9 99 else {
arnaudsuire 0:d58c1b8d63d9 100 tempTauR = (1.0 / tauI) * tSample_;
arnaudsuire 0:d58c1b8d63d9 101 }
arnaudsuire 0:d58c1b8d63d9 102
arnaudsuire 0:d58c1b8d63d9 103 //For "bumpless transfer" we need to rescale the accumulated error.
arnaudsuire 0:d58c1b8d63d9 104 //if (inAuto) {
arnaudsuire 0:d58c1b8d63d9 105 //if (tempTauR == 0.0) {
arnaudsuire 0:d58c1b8d63d9 106 //accError_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 107 //}
arnaudsuire 0:d58c1b8d63d9 108 //else {
arnaudsuire 0:d58c1b8d63d9 109 accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
arnaudsuire 0:d58c1b8d63d9 110 //}
arnaudsuire 0:d58c1b8d63d9 111 //}
arnaudsuire 0:d58c1b8d63d9 112
arnaudsuire 0:d58c1b8d63d9 113 Kc_ = Kc;
arnaudsuire 0:d58c1b8d63d9 114 tauR_ = tempTauR;
arnaudsuire 0:d58c1b8d63d9 115 tauD_ = tauD / tSample_;
arnaudsuire 0:d58c1b8d63d9 116
arnaudsuire 0:d58c1b8d63d9 117 }
arnaudsuire 0:d58c1b8d63d9 118
arnaudsuire 0:d58c1b8d63d9 119 void PID::reset(void) {
arnaudsuire 0:d58c1b8d63d9 120
arnaudsuire 0:d58c1b8d63d9 121 float scaledBias = 0.0;
arnaudsuire 0:d58c1b8d63d9 122
arnaudsuire 0:d58c1b8d63d9 123 if (usingFeedForward) {
arnaudsuire 0:d58c1b8d63d9 124 scaledBias = (bias_ - outMin_) / outSpan_;
arnaudsuire 0:d58c1b8d63d9 125 }
arnaudsuire 0:d58c1b8d63d9 126 else {
arnaudsuire 0:d58c1b8d63d9 127 scaledBias = (realOutput_ - outMin_) / outSpan_;
arnaudsuire 0:d58c1b8d63d9 128 }
arnaudsuire 0:d58c1b8d63d9 129
arnaudsuire 0:d58c1b8d63d9 130 prevControllerOutput_ = scaledBias;
arnaudsuire 0:d58c1b8d63d9 131 prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_;
arnaudsuire 0:d58c1b8d63d9 132
arnaudsuire 0:d58c1b8d63d9 133 //Clear any error in the integral.
arnaudsuire 0:d58c1b8d63d9 134 accError_ = 0;
arnaudsuire 0:d58c1b8d63d9 135
arnaudsuire 0:d58c1b8d63d9 136 }
unix_guru 1:117e0c36eb22 137
arnaudsuire 0:d58c1b8d63d9 138 void PID::setMode(int mode) {
arnaudsuire 0:d58c1b8d63d9 139
arnaudsuire 0:d58c1b8d63d9 140 //We were in manual, and we just got set to auto.
arnaudsuire 0:d58c1b8d63d9 141 //Reset the controller internals.
arnaudsuire 0:d58c1b8d63d9 142 if (mode != 0 && !inAuto) {
arnaudsuire 0:d58c1b8d63d9 143 reset();
arnaudsuire 0:d58c1b8d63d9 144 }
arnaudsuire 0:d58c1b8d63d9 145
arnaudsuire 0:d58c1b8d63d9 146 inAuto = (mode != 0);
arnaudsuire 0:d58c1b8d63d9 147
unix_guru 1:117e0c36eb22 148 }
arnaudsuire 0:d58c1b8d63d9 149
arnaudsuire 0:d58c1b8d63d9 150 void PID::setInterval(float interval) {
arnaudsuire 0:d58c1b8d63d9 151
arnaudsuire 0:d58c1b8d63d9 152 if (interval > 0) {
arnaudsuire 0:d58c1b8d63d9 153 //Convert the time-based tunings to reflect this change.
arnaudsuire 0:d58c1b8d63d9 154 tauR_ *= (interval / tSample_);
arnaudsuire 0:d58c1b8d63d9 155 accError_ *= (tSample_ / interval);
arnaudsuire 0:d58c1b8d63d9 156 tauD_ *= (interval / tSample_);
arnaudsuire 0:d58c1b8d63d9 157 tSample_ = interval;
arnaudsuire 0:d58c1b8d63d9 158 }
arnaudsuire 0:d58c1b8d63d9 159
arnaudsuire 0:d58c1b8d63d9 160 }
unix_guru 1:117e0c36eb22 161
arnaudsuire 0:d58c1b8d63d9 162 void PID::setSetPoint(float sp) {
arnaudsuire 0:d58c1b8d63d9 163
arnaudsuire 0:d58c1b8d63d9 164 setPoint_ = sp;
arnaudsuire 0:d58c1b8d63d9 165
arnaudsuire 0:d58c1b8d63d9 166 }
arnaudsuire 0:d58c1b8d63d9 167
arnaudsuire 0:d58c1b8d63d9 168 void PID::setProcessValue(float pv) {
arnaudsuire 0:d58c1b8d63d9 169
arnaudsuire 0:d58c1b8d63d9 170 processVariable_ = pv;
arnaudsuire 0:d58c1b8d63d9 171
arnaudsuire 0:d58c1b8d63d9 172 }
unix_guru 1:117e0c36eb22 173
arnaudsuire 0:d58c1b8d63d9 174 void PID::setBias(float bias){
arnaudsuire 0:d58c1b8d63d9 175
arnaudsuire 0:d58c1b8d63d9 176 bias_ = bias;
arnaudsuire 0:d58c1b8d63d9 177 usingFeedForward = 1;
arnaudsuire 0:d58c1b8d63d9 178
arnaudsuire 0:d58c1b8d63d9 179 }
arnaudsuire 0:d58c1b8d63d9 180
arnaudsuire 0:d58c1b8d63d9 181 float PID::compute(float pv, float sp) {
arnaudsuire 0:d58c1b8d63d9 182
arnaudsuire 0:d58c1b8d63d9 183 //enregistrer variables dans var interne
arnaudsuire 0:d58c1b8d63d9 184 processVariable_ = pv; //ce que l'on mesure
arnaudsuire 0:d58c1b8d63d9 185 setPoint_ = sp; // ce que l'on veut atteindre
arnaudsuire 0:d58c1b8d63d9 186
arnaudsuire 0:d58c1b8d63d9 187 //Pull in the input and setpoint, and scale them into percent span.
arnaudsuire 0:d58c1b8d63d9 188 float scaledPV = (processVariable_ - inMin_) / inSpan_;
arnaudsuire 0:d58c1b8d63d9 189
arnaudsuire 0:d58c1b8d63d9 190 if (scaledPV > 1.0) {
arnaudsuire 0:d58c1b8d63d9 191 scaledPV = 1.0;
arnaudsuire 0:d58c1b8d63d9 192 }
arnaudsuire 0:d58c1b8d63d9 193 else if (scaledPV < 0.0) {
arnaudsuire 0:d58c1b8d63d9 194 scaledPV = 0.0;
arnaudsuire 0:d58c1b8d63d9 195 }
arnaudsuire 0:d58c1b8d63d9 196
arnaudsuire 0:d58c1b8d63d9 197 float scaledSP = (setPoint_ - inMin_) / inSpan_;
arnaudsuire 0:d58c1b8d63d9 198 if (scaledSP > 1.0) {
arnaudsuire 0:d58c1b8d63d9 199 scaledSP = 1;
arnaudsuire 0:d58c1b8d63d9 200 }
arnaudsuire 0:d58c1b8d63d9 201 else if (scaledSP < 0.0) {
arnaudsuire 0:d58c1b8d63d9 202 scaledSP = 0;
arnaudsuire 0:d58c1b8d63d9 203 }
arnaudsuire 0:d58c1b8d63d9 204
arnaudsuire 0:d58c1b8d63d9 205 float error = scaledSP - scaledPV;
arnaudsuire 0:d58c1b8d63d9 206
arnaudsuire 0:d58c1b8d63d9 207 //Check and see if the output is pegged at a limit and only
arnaudsuire 0:d58c1b8d63d9 208 //integrate if it is not. This is to prevent reset-windup.
arnaudsuire 0:d58c1b8d63d9 209 if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
arnaudsuire 0:d58c1b8d63d9 210 accError_ += error;
arnaudsuire 0:d58c1b8d63d9 211 }
arnaudsuire 0:d58c1b8d63d9 212
arnaudsuire 0:d58c1b8d63d9 213 //Compute the current slope of the input signal.
arnaudsuire 0:d58c1b8d63d9 214 float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
arnaudsuire 0:d58c1b8d63d9 215 //float dMeas = (scaledPV - prevProcessVariable_);
arnaudsuire 0:d58c1b8d63d9 216
arnaudsuire 0:d58c1b8d63d9 217 float scaledBias = 0.0;
arnaudsuire 0:d58c1b8d63d9 218
arnaudsuire 0:d58c1b8d63d9 219 if (usingFeedForward) {
arnaudsuire 0:d58c1b8d63d9 220 scaledBias = (bias_ - outMin_) / outSpan_;
arnaudsuire 0:d58c1b8d63d9 221 }
arnaudsuire 0:d58c1b8d63d9 222
arnaudsuire 0:d58c1b8d63d9 223 //Perform the PID calculation.
arnaudsuire 0:d58c1b8d63d9 224 controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
arnaudsuire 0:d58c1b8d63d9 225 //controllerOutput_ = Kc_ * error + tauR_ * accError_ + tauD_ * dMeas;
arnaudsuire 0:d58c1b8d63d9 226
arnaudsuire 0:d58c1b8d63d9 227 //Make sure the computed output is within output constraints.
arnaudsuire 0:d58c1b8d63d9 228 if (controllerOutput_ < 0.0) {
arnaudsuire 0:d58c1b8d63d9 229 controllerOutput_ = 0.0;
arnaudsuire 0:d58c1b8d63d9 230 }
arnaudsuire 0:d58c1b8d63d9 231 else if (controllerOutput_ > 1.0) {
arnaudsuire 0:d58c1b8d63d9 232 controllerOutput_ = 1.0;
arnaudsuire 0:d58c1b8d63d9 233 }
arnaudsuire 0:d58c1b8d63d9 234
arnaudsuire 0:d58c1b8d63d9 235 //Remember this output for the windup check next time.
arnaudsuire 0:d58c1b8d63d9 236 prevControllerOutput_ = controllerOutput_;
arnaudsuire 0:d58c1b8d63d9 237 //Remember the input for the derivative calculation next time.
arnaudsuire 0:d58c1b8d63d9 238 prevProcessVariable_ = scaledPV;
arnaudsuire 0:d58c1b8d63d9 239
arnaudsuire 0:d58c1b8d63d9 240 //Scale the output from percent span back out to a real world number.
arnaudsuire 0:d58c1b8d63d9 241 return ((controllerOutput_ * outSpan_) + outMin_);
arnaudsuire 0:d58c1b8d63d9 242
arnaudsuire 0:d58c1b8d63d9 243 }
arnaudsuire 0:d58c1b8d63d9 244
arnaudsuire 0:d58c1b8d63d9 245 float PID::getInMin() {
arnaudsuire 0:d58c1b8d63d9 246
arnaudsuire 0:d58c1b8d63d9 247 return inMin_;
arnaudsuire 0:d58c1b8d63d9 248
arnaudsuire 0:d58c1b8d63d9 249 }
arnaudsuire 0:d58c1b8d63d9 250
arnaudsuire 0:d58c1b8d63d9 251 float PID::getInMax() {
arnaudsuire 0:d58c1b8d63d9 252
arnaudsuire 0:d58c1b8d63d9 253 return inMax_;
arnaudsuire 0:d58c1b8d63d9 254
arnaudsuire 0:d58c1b8d63d9 255 }
arnaudsuire 0:d58c1b8d63d9 256
arnaudsuire 0:d58c1b8d63d9 257 float PID::getOutMin() {
arnaudsuire 0:d58c1b8d63d9 258
arnaudsuire 0:d58c1b8d63d9 259 return outMin_;
arnaudsuire 0:d58c1b8d63d9 260
arnaudsuire 0:d58c1b8d63d9 261 }
arnaudsuire 0:d58c1b8d63d9 262
arnaudsuire 0:d58c1b8d63d9 263 float PID::getOutMax() {
arnaudsuire 0:d58c1b8d63d9 264
arnaudsuire 0:d58c1b8d63d9 265 return outMax_;
arnaudsuire 0:d58c1b8d63d9 266
arnaudsuire 0:d58c1b8d63d9 267 }
arnaudsuire 0:d58c1b8d63d9 268
arnaudsuire 0:d58c1b8d63d9 269 float PID::getInterval() {
arnaudsuire 0:d58c1b8d63d9 270
arnaudsuire 0:d58c1b8d63d9 271 return tSample_;
arnaudsuire 0:d58c1b8d63d9 272
arnaudsuire 0:d58c1b8d63d9 273 }
arnaudsuire 0:d58c1b8d63d9 274
arnaudsuire 0:d58c1b8d63d9 275 float PID::getPParam() {
arnaudsuire 0:d58c1b8d63d9 276
arnaudsuire 0:d58c1b8d63d9 277 return pParam_;
arnaudsuire 0:d58c1b8d63d9 278
arnaudsuire 0:d58c1b8d63d9 279 }
arnaudsuire 0:d58c1b8d63d9 280
arnaudsuire 0:d58c1b8d63d9 281 float PID::getIParam() {
arnaudsuire 0:d58c1b8d63d9 282
arnaudsuire 0:d58c1b8d63d9 283 return iParam_;
arnaudsuire 0:d58c1b8d63d9 284
arnaudsuire 0:d58c1b8d63d9 285 }
arnaudsuire 0:d58c1b8d63d9 286
arnaudsuire 0:d58c1b8d63d9 287 float PID::getDParam() {
arnaudsuire 0:d58c1b8d63d9 288
arnaudsuire 0:d58c1b8d63d9 289 return dParam_;
arnaudsuire 0:d58c1b8d63d9 290
arnaudsuire 0:d58c1b8d63d9 291 }