kao yi / Mbed 2 deprecated Boboobooov4

wu

Dependencies:   mbed-rtos mbed

Fork of Bov3 by kao yi

controller.cpp@7:fd976e1ced33, 2014-06-22 (annotated)

Committer:
backman
Date:
Sun Jun 22 13:58:01 2014 +0000
Revision:
7:fd976e1ced33
Child:
8:8e49e21d80a2
printf should use right type

Who changed what in which revision?

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