Tony Lin / Mbed 2 deprecated BX-car

hello

Dependencies:   mbed-rtos mbed

Fork of BX-car by kao yi

controller.cpp@10:9f0ce6ba7663, 2014-06-26 (annotated)

Committer:
TonyLin
Date:
Thu Jun 26 14:29:53 2014 +0000
Revision:
10:9f0ce6ba7663
Parent:
9:33b99cb45e99 
ok;

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