Shuto Naruse / Mbed 2 deprecated Eurobot2012_Secondary

Eurobot2012_Secondary

Fork of Eurobot_2012_Secondary by Shuto Naruse

PID/PID.cpp@1:cc2a9eb0bd55, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:25:31 2012 +0000
Revision:
1:cc2a9eb0bd55
Commit before publishing

Who changed what in which revision?

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