Hang Liu / PID

Fork of PID by Kamil Foryszewski

PID.cpp@1:0ffb635770b3, 2016-11-03 (annotated)

Committer:
kfforex
Date:
Thu Nov 03 17:15:10 2016 +0000
Revision:
1:0ffb635770b3
Parent:
0:6e12a3e5af19
Child:
2:0fff4827f3b6
before publish

Who changed what in which revision?

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