Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

PID/PID.cpp@26:0995f61cb7b8, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:22:47 2012 +0000
Revision:
26:0995f61cb7b8
Parent:
25:143b19c1fb05 
Eurobot 2012 Primary;

Who changed what in which revision?

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