Buggy bois / Mbed 2 deprecated WORKING_TRIAL

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

PID/PID.cpp@3:01b5e80d842d, 2019-03-09 (annotated)

Committer:
mazdo25
Date:
Sat Mar 09 14:27:48 2019 +0000
Revision:
3:01b5e80d842d
Parent:
2:730ccfbf08d5 
Initialization working, sensors not

Who changed what in which revision?

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