quadcopter cufe / Mbed 2 deprecated FollowWall

follow wall 1

Dependencies:   mbed

PID/PID.cpp@0:b703833f6795, 2014-04-08 (annotated)

Committer:
khaledelmadawi
Date:
Tue Apr 08 13:23:06 2014 +0000
Revision:
0:b703833f6795
follow wall 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:b703833f6795 1 /**
khaledelmadawi 0:b703833f6795 2 * @author Aaron Berk
khaledelmadawi 0:b703833f6795 3 *
khaledelmadawi 0:b703833f6795 4 * @section LICENSE
khaledelmadawi 0:b703833f6795 5 *
khaledelmadawi 0:b703833f6795 6 * Copyright (c) 2010 ARM Limited
khaledelmadawi 0:b703833f6795 7 *
khaledelmadawi 0:b703833f6795 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
khaledelmadawi 0:b703833f6795 9 * of this software and associated documentation files (the "Software"), to deal
khaledelmadawi 0:b703833f6795 10 * in the Software without restriction, including without limitation the rights
khaledelmadawi 0:b703833f6795 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
khaledelmadawi 0:b703833f6795 12 * copies of the Software, and to permit persons to whom the Software is
khaledelmadawi 0:b703833f6795 13 * furnished to do so, subject to the following conditions:
khaledelmadawi 0:b703833f6795 14 *
khaledelmadawi 0:b703833f6795 15 * The above copyright notice and this permission notice shall be included in
khaledelmadawi 0:b703833f6795 16 * all copies or substantial portions of the Software.
khaledelmadawi 0:b703833f6795 17 *
khaledelmadawi 0:b703833f6795 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
khaledelmadawi 0:b703833f6795 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
khaledelmadawi 0:b703833f6795 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
khaledelmadawi 0:b703833f6795 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
khaledelmadawi 0:b703833f6795 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
khaledelmadawi 0:b703833f6795 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
khaledelmadawi 0:b703833f6795 24 * THE SOFTWARE.
khaledelmadawi 0:b703833f6795 25 *
khaledelmadawi 0:b703833f6795 26 * @section DESCRIPTION
khaledelmadawi 0:b703833f6795 27 *
khaledelmadawi 0:b703833f6795 28 * A PID controller is a widely used feedback controller commonly found in
khaledelmadawi 0:b703833f6795 29 * industry.
khaledelmadawi 0:b703833f6795 30 *
khaledelmadawi 0:b703833f6795 31 * This library is a port of Brett Beauregard's Arduino PID library:
khaledelmadawi 0:b703833f6795 32 *
khaledelmadawi 0:b703833f6795 33 * http://www.arduino.cc/playground/Code/PIDLibrary
khaledelmadawi 0:b703833f6795 34 *
khaledelmadawi 0:b703833f6795 35 * The wikipedia article on PID controllers is a good place to start on
khaledelmadawi 0:b703833f6795 36 * understanding how they work:
khaledelmadawi 0:b703833f6795 37 *
khaledelmadawi 0:b703833f6795 38 * http://en.wikipedia.org/wiki/PID_controller
khaledelmadawi 0:b703833f6795 39 *
khaledelmadawi 0:b703833f6795 40 * For a clear and elegant explanation of how to implement and tune a
khaledelmadawi 0:b703833f6795 41 * controller, the controlguru website by Douglas J. Cooper (who also happened
khaledelmadawi 0:b703833f6795 42 * to be Brett's controls professor) is an excellent reference:
khaledelmadawi 0:b703833f6795 43 *
khaledelmadawi 0:b703833f6795 44 * http://www.controlguru.com/
khaledelmadawi 0:b703833f6795 45 */
khaledelmadawi 0:b703833f6795 46
khaledelmadawi 0:b703833f6795 47 /**
khaledelmadawi 0:b703833f6795 48 * Includes
khaledelmadawi 0:b703833f6795 49 */
khaledelmadawi 0:b703833f6795 50 #include "PID.h"
khaledelmadawi 0:b703833f6795 51
khaledelmadawi 0:b703833f6795 52 PID::PID(float Kc, float tauI, float tauD, float interval) {
khaledelmadawi 0:b703833f6795 53
khaledelmadawi 0:b703833f6795 54 usingFeedForward = false;
khaledelmadawi 0:b703833f6795 55 inAuto = false;
khaledelmadawi 0:b703833f6795 56
khaledelmadawi 0:b703833f6795 57 //Default the limits to the full range of I/O: 3.3V
khaledelmadawi 0:b703833f6795 58 //Make sure to set these to more appropriate limits for
khaledelmadawi 0:b703833f6795 59 //your application.
khaledelmadawi 0:b703833f6795 60 setInputLimits(0.0, 3.3);
khaledelmadawi 0:b703833f6795 61 setOutputLimits(0.0, 3.3);
khaledelmadawi 0:b703833f6795 62
khaledelmadawi 0:b703833f6795 63 tSample_ = interval;
khaledelmadawi 0:b703833f6795 64
khaledelmadawi 0:b703833f6795 65 setTunings(Kc, tauI, tauD);
khaledelmadawi 0:b703833f6795 66
khaledelmadawi 0:b703833f6795 67 setPoint_ = 0.0;
khaledelmadawi 0:b703833f6795 68 processVariable_ = 0.0;
khaledelmadawi 0:b703833f6795 69 prevProcessVariable_ = 0.0;
khaledelmadawi 0:b703833f6795 70 controllerOutput_ = 0.0;
khaledelmadawi 0:b703833f6795 71 prevControllerOutput_ = 0.0;
khaledelmadawi 0:b703833f6795 72
khaledelmadawi 0:b703833f6795 73 accError_ = 0.0;
khaledelmadawi 0:b703833f6795 74 bias_ = 0.0;
khaledelmadawi 0:b703833f6795 75
khaledelmadawi 0:b703833f6795 76 realOutput_ = 0.0;
khaledelmadawi 0:b703833f6795 77
khaledelmadawi 0:b703833f6795 78 }
khaledelmadawi 0:b703833f6795 79
khaledelmadawi 0:b703833f6795 80 void PID::setInputLimits(float inMin, float inMax) {
khaledelmadawi 0:b703833f6795 81
khaledelmadawi 0:b703833f6795 82 //Make sure we haven't been given impossible values.
khaledelmadawi 0:b703833f6795 83 if (inMin >= inMax) {
khaledelmadawi 0:b703833f6795 84 return;
khaledelmadawi 0:b703833f6795 85 }
khaledelmadawi 0:b703833f6795 86
khaledelmadawi 0:b703833f6795 87 //Rescale the working variables to reflect the changes.
khaledelmadawi 0:b703833f6795 88 prevProcessVariable_ *= (inMax - inMin) / inSpan_;
khaledelmadawi 0:b703833f6795 89 accError_ *= (inMax - inMin) / inSpan_;
khaledelmadawi 0:b703833f6795 90
khaledelmadawi 0:b703833f6795 91 //Make sure the working variables are within the new limits.
khaledelmadawi 0:b703833f6795 92 if (prevProcessVariable_ > 1) {
khaledelmadawi 0:b703833f6795 93 prevProcessVariable_ = 1;
khaledelmadawi 0:b703833f6795 94 } else if (prevProcessVariable_ < 0) {
khaledelmadawi 0:b703833f6795 95 prevProcessVariable_ = 0;
khaledelmadawi 0:b703833f6795 96 }
khaledelmadawi 0:b703833f6795 97
khaledelmadawi 0:b703833f6795 98 inMin_ = inMin;
khaledelmadawi 0:b703833f6795 99 inMax_ = inMax;
khaledelmadawi 0:b703833f6795 100 inSpan_ = inMax - inMin;
khaledelmadawi 0:b703833f6795 101
khaledelmadawi 0:b703833f6795 102 }
khaledelmadawi 0:b703833f6795 103
khaledelmadawi 0:b703833f6795 104 void PID::setOutputLimits(float outMin, float outMax) {
khaledelmadawi 0:b703833f6795 105
khaledelmadawi 0:b703833f6795 106 //Make sure we haven't been given impossible values.
khaledelmadawi 0:b703833f6795 107 if (outMin >= outMax) {
khaledelmadawi 0:b703833f6795 108 return;
khaledelmadawi 0:b703833f6795 109 }
khaledelmadawi 0:b703833f6795 110
khaledelmadawi 0:b703833f6795 111 //Rescale the working variables to reflect the changes.
khaledelmadawi 0:b703833f6795 112 prevControllerOutput_ *= (outMax - outMin) / outSpan_;
khaledelmadawi 0:b703833f6795 113
khaledelmadawi 0:b703833f6795 114 //Make sure the working variables are within the new limits.
khaledelmadawi 0:b703833f6795 115 if (prevControllerOutput_ > 1) {
khaledelmadawi 0:b703833f6795 116 prevControllerOutput_ = 1;
khaledelmadawi 0:b703833f6795 117 } else if (prevControllerOutput_ < 0) {
khaledelmadawi 0:b703833f6795 118 prevControllerOutput_ = 0;
khaledelmadawi 0:b703833f6795 119 }
khaledelmadawi 0:b703833f6795 120
khaledelmadawi 0:b703833f6795 121 outMin_ = outMin;
khaledelmadawi 0:b703833f6795 122 outMax_ = outMax;
khaledelmadawi 0:b703833f6795 123 outSpan_ = outMax - outMin;
khaledelmadawi 0:b703833f6795 124
khaledelmadawi 0:b703833f6795 125 }
khaledelmadawi 0:b703833f6795 126
khaledelmadawi 0:b703833f6795 127 void PID::setTunings(float Kc, float tauI, float tauD) {
khaledelmadawi 0:b703833f6795 128
khaledelmadawi 0:b703833f6795 129 //Verify that the tunings make sense.
khaledelmadawi 0:b703833f6795 130 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
khaledelmadawi 0:b703833f6795 131 return;
khaledelmadawi 0:b703833f6795 132 }
khaledelmadawi 0:b703833f6795 133
khaledelmadawi 0:b703833f6795 134 //Store raw values to hand back to user on request.
khaledelmadawi 0:b703833f6795 135 pParam_ = Kc;
khaledelmadawi 0:b703833f6795 136 iParam_ = tauI;
khaledelmadawi 0:b703833f6795 137 dParam_ = tauD;
khaledelmadawi 0:b703833f6795 138
khaledelmadawi 0:b703833f6795 139 float tempTauR;
khaledelmadawi 0:b703833f6795 140
khaledelmadawi 0:b703833f6795 141 if (tauI == 0.0) {
khaledelmadawi 0:b703833f6795 142 tempTauR = 0.0;
khaledelmadawi 0:b703833f6795 143 } else {
khaledelmadawi 0:b703833f6795 144 tempTauR = (1.0 / tauI) * tSample_;
khaledelmadawi 0:b703833f6795 145 }
khaledelmadawi 0:b703833f6795 146
khaledelmadawi 0:b703833f6795 147 //For "bumpless transfer" we need to rescale the accumulated error.
khaledelmadawi 0:b703833f6795 148 if (inAuto) {
khaledelmadawi 0:b703833f6795 149 if (tempTauR == 0.0) {
khaledelmadawi 0:b703833f6795 150 accError_ = 0.0;
khaledelmadawi 0:b703833f6795 151 } else {
khaledelmadawi 0:b703833f6795 152 accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
khaledelmadawi 0:b703833f6795 153 }
khaledelmadawi 0:b703833f6795 154 }
khaledelmadawi 0:b703833f6795 155
khaledelmadawi 0:b703833f6795 156 Kc_ = Kc;
khaledelmadawi 0:b703833f6795 157 tauR_ = tempTauR;
khaledelmadawi 0:b703833f6795 158 tauD_ = tauD / tSample_;
khaledelmadawi 0:b703833f6795 159
khaledelmadawi 0:b703833f6795 160 }
khaledelmadawi 0:b703833f6795 161
khaledelmadawi 0:b703833f6795 162 void PID::reset(void) {
khaledelmadawi 0:b703833f6795 163
khaledelmadawi 0:b703833f6795 164 float scaledBias = 0.0;
khaledelmadawi 0:b703833f6795 165
khaledelmadawi 0:b703833f6795 166 if (usingFeedForward) {
khaledelmadawi 0:b703833f6795 167 scaledBias = (bias_ - outMin_) / outSpan_;
khaledelmadawi 0:b703833f6795 168 } else {
khaledelmadawi 0:b703833f6795 169 scaledBias = (realOutput_ - outMin_) / outSpan_;
khaledelmadawi 0:b703833f6795 170 }
khaledelmadawi 0:b703833f6795 171
khaledelmadawi 0:b703833f6795 172 prevControllerOutput_ = scaledBias;
khaledelmadawi 0:b703833f6795 173 prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_;
khaledelmadawi 0:b703833f6795 174
khaledelmadawi 0:b703833f6795 175 //Clear any error in the integral.
khaledelmadawi 0:b703833f6795 176 accError_ = 0;
khaledelmadawi 0:b703833f6795 177
khaledelmadawi 0:b703833f6795 178 }
khaledelmadawi 0:b703833f6795 179
khaledelmadawi 0:b703833f6795 180 void PID::setMode(int mode) {
khaledelmadawi 0:b703833f6795 181
khaledelmadawi 0:b703833f6795 182 //We were in manual, and we just got set to auto.
khaledelmadawi 0:b703833f6795 183 //Reset the controller internals.
khaledelmadawi 0:b703833f6795 184 if (mode != 0 && !inAuto) {
khaledelmadawi 0:b703833f6795 185 reset();
khaledelmadawi 0:b703833f6795 186 }
khaledelmadawi 0:b703833f6795 187
khaledelmadawi 0:b703833f6795 188 inAuto = (mode != 0);
khaledelmadawi 0:b703833f6795 189
khaledelmadawi 0:b703833f6795 190 }
khaledelmadawi 0:b703833f6795 191
khaledelmadawi 0:b703833f6795 192 void PID::setInterval(float interval) {
khaledelmadawi 0:b703833f6795 193
khaledelmadawi 0:b703833f6795 194 if (interval > 0) {
khaledelmadawi 0:b703833f6795 195 //Convert the time-based tunings to reflect this change.
khaledelmadawi 0:b703833f6795 196 tauR_ *= (interval / tSample_);
khaledelmadawi 0:b703833f6795 197 accError_ *= (tSample_ / interval);
khaledelmadawi 0:b703833f6795 198 tauD_ *= (interval / tSample_);
khaledelmadawi 0:b703833f6795 199 tSample_ = interval;
khaledelmadawi 0:b703833f6795 200 }
khaledelmadawi 0:b703833f6795 201
khaledelmadawi 0:b703833f6795 202 }
khaledelmadawi 0:b703833f6795 203
khaledelmadawi 0:b703833f6795 204 void PID::setSetPoint(float sp) {
khaledelmadawi 0:b703833f6795 205
khaledelmadawi 0:b703833f6795 206 setPoint_ = sp;
khaledelmadawi 0:b703833f6795 207
khaledelmadawi 0:b703833f6795 208 }
khaledelmadawi 0:b703833f6795 209
khaledelmadawi 0:b703833f6795 210 void PID::setProcessValue(float pv) {
khaledelmadawi 0:b703833f6795 211
khaledelmadawi 0:b703833f6795 212 processVariable_ = pv;
khaledelmadawi 0:b703833f6795 213
khaledelmadawi 0:b703833f6795 214 }
khaledelmadawi 0:b703833f6795 215
khaledelmadawi 0:b703833f6795 216 void PID::setBias(float bias){
khaledelmadawi 0:b703833f6795 217
khaledelmadawi 0:b703833f6795 218 bias_ = bias;
khaledelmadawi 0:b703833f6795 219 usingFeedForward = 1;
khaledelmadawi 0:b703833f6795 220
khaledelmadawi 0:b703833f6795 221 }
khaledelmadawi 0:b703833f6795 222
khaledelmadawi 0:b703833f6795 223 float PID::compute() {
khaledelmadawi 0:b703833f6795 224
khaledelmadawi 0:b703833f6795 225 //Pull in the input and setpoint, and scale them into percent span.
khaledelmadawi 0:b703833f6795 226 float scaledPV = (processVariable_ - inMin_) / inSpan_;
khaledelmadawi 0:b703833f6795 227
khaledelmadawi 0:b703833f6795 228 if (scaledPV > 1.0) {
khaledelmadawi 0:b703833f6795 229 scaledPV = 1.0;
khaledelmadawi 0:b703833f6795 230 } else if (scaledPV < 0.0) {
khaledelmadawi 0:b703833f6795 231 scaledPV = 0.0;
khaledelmadawi 0:b703833f6795 232 }
khaledelmadawi 0:b703833f6795 233
khaledelmadawi 0:b703833f6795 234 float scaledSP = (setPoint_ - inMin_) / inSpan_;
khaledelmadawi 0:b703833f6795 235 if (scaledSP > 1.0) {
khaledelmadawi 0:b703833f6795 236 scaledSP = 1;
khaledelmadawi 0:b703833f6795 237 } else if (scaledSP < 0.0) {
khaledelmadawi 0:b703833f6795 238 scaledSP = 0;
khaledelmadawi 0:b703833f6795 239 }
khaledelmadawi 0:b703833f6795 240
khaledelmadawi 0:b703833f6795 241 float error = scaledSP - scaledPV;
khaledelmadawi 0:b703833f6795 242
khaledelmadawi 0:b703833f6795 243 //Check and see if the output is pegged at a limit and only
khaledelmadawi 0:b703833f6795 244 //integrate if it is not. This is to prevent reset-windup.
khaledelmadawi 0:b703833f6795 245 if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
khaledelmadawi 0:b703833f6795 246 accError_ += error;
khaledelmadawi 0:b703833f6795 247 }
khaledelmadawi 0:b703833f6795 248
khaledelmadawi 0:b703833f6795 249 //Compute the current slope of the input signal.
khaledelmadawi 0:b703833f6795 250 float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
khaledelmadawi 0:b703833f6795 251
khaledelmadawi 0:b703833f6795 252 float scaledBias = 0.0;
khaledelmadawi 0:b703833f6795 253
khaledelmadawi 0:b703833f6795 254 if (usingFeedForward) {
khaledelmadawi 0:b703833f6795 255 scaledBias = (bias_ - outMin_) / outSpan_;
khaledelmadawi 0:b703833f6795 256 }
khaledelmadawi 0:b703833f6795 257
khaledelmadawi 0:b703833f6795 258 //Perform the PID calculation.
khaledelmadawi 0:b703833f6795 259 controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
khaledelmadawi 0:b703833f6795 260
khaledelmadawi 0:b703833f6795 261 //Make sure the computed output is within output constraints.
khaledelmadawi 0:b703833f6795 262 if (controllerOutput_ < 0.0) {
khaledelmadawi 0:b703833f6795 263 controllerOutput_ = 0.0;
khaledelmadawi 0:b703833f6795 264 } else if (controllerOutput_ > 1.0) {
khaledelmadawi 0:b703833f6795 265 controllerOutput_ = 1.0;
khaledelmadawi 0:b703833f6795 266 }
khaledelmadawi 0:b703833f6795 267
khaledelmadawi 0:b703833f6795 268 //Remember this output for the windup check next time.
khaledelmadawi 0:b703833f6795 269 prevControllerOutput_ = controllerOutput_;
khaledelmadawi 0:b703833f6795 270 //Remember the input for the derivative calculation next time.
khaledelmadawi 0:b703833f6795 271 prevProcessVariable_ = scaledPV;
khaledelmadawi 0:b703833f6795 272
khaledelmadawi 0:b703833f6795 273 //Scale the output from percent span back out to a real world number.
khaledelmadawi 0:b703833f6795 274 return ((controllerOutput_ * outSpan_) + outMin_);
khaledelmadawi 0:b703833f6795 275
khaledelmadawi 0:b703833f6795 276 }
khaledelmadawi 0:b703833f6795 277
khaledelmadawi 0:b703833f6795 278 float PID::getInMin() {
khaledelmadawi 0:b703833f6795 279
khaledelmadawi 0:b703833f6795 280 return inMin_;
khaledelmadawi 0:b703833f6795 281
khaledelmadawi 0:b703833f6795 282 }
khaledelmadawi 0:b703833f6795 283
khaledelmadawi 0:b703833f6795 284 float PID::getInMax() {
khaledelmadawi 0:b703833f6795 285
khaledelmadawi 0:b703833f6795 286 return inMax_;
khaledelmadawi 0:b703833f6795 287
khaledelmadawi 0:b703833f6795 288 }
khaledelmadawi 0:b703833f6795 289
khaledelmadawi 0:b703833f6795 290 float PID::getOutMin() {
khaledelmadawi 0:b703833f6795 291
khaledelmadawi 0:b703833f6795 292 return outMin_;
khaledelmadawi 0:b703833f6795 293
khaledelmadawi 0:b703833f6795 294 }
khaledelmadawi 0:b703833f6795 295
khaledelmadawi 0:b703833f6795 296 float PID::getOutMax() {
khaledelmadawi 0:b703833f6795 297
khaledelmadawi 0:b703833f6795 298 return outMax_;
khaledelmadawi 0:b703833f6795 299
khaledelmadawi 0:b703833f6795 300 }
khaledelmadawi 0:b703833f6795 301
khaledelmadawi 0:b703833f6795 302 float PID::getInterval() {
khaledelmadawi 0:b703833f6795 303
khaledelmadawi 0:b703833f6795 304 return tSample_;
khaledelmadawi 0:b703833f6795 305
khaledelmadawi 0:b703833f6795 306 }
khaledelmadawi 0:b703833f6795 307
khaledelmadawi 0:b703833f6795 308 float PID::getPParam() {
khaledelmadawi 0:b703833f6795 309
khaledelmadawi 0:b703833f6795 310 return pParam_;
khaledelmadawi 0:b703833f6795 311
khaledelmadawi 0:b703833f6795 312 }
khaledelmadawi 0:b703833f6795 313
khaledelmadawi 0:b703833f6795 314 float PID::getIParam() {
khaledelmadawi 0:b703833f6795 315
khaledelmadawi 0:b703833f6795 316 return iParam_;
khaledelmadawi 0:b703833f6795 317
khaledelmadawi 0:b703833f6795 318 }
khaledelmadawi 0:b703833f6795 319
khaledelmadawi 0:b703833f6795 320 float PID::getDParam() {
khaledelmadawi 0:b703833f6795 321
khaledelmadawi 0:b703833f6795 322 return dParam_;
khaledelmadawi 0:b703833f6795 323
khaledelmadawi 0:b703833f6795 324 }