quadcopter cufe / Mbed 2 deprecated FollowWall2

Go to goal robot with follow wall algo

Dependencies:   mbed

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

Committer:
khaledelmadawi
Date:
Tue Apr 08 13:21:17 2014 +0000
Revision:
0:efef62b55c86
Follow wall

Who changed what in which revision?

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