Arnaud Suire / PID

pid

Dependents:   Labo_TRSE_Drone

PID.cpp@0:d58c1b8d63d9, 2014-02-26 (annotated)

Committer:
arnaudsuire
Date:
Wed Feb 26 08:46:04 2014 +0000
Revision:
0:d58c1b8d63d9
arnaud pid giro acc

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