frscl / PID

Proportional, integral, derivative controller library. Ported from the Arduino PID library by Brett Beauregard.

Fork of PID by Aaron Berk

PID.cpp@3:0f1bc3b0d050, 2015-05-19 (annotated)

Committer:
miczyg
Date:
Tue May 19 16:39:18 2015 +0000
Revision:
3:0f1bc3b0d050
Parent:
1:709ed452cc4c 
Wzmocnienie, period itd ok, wersja P dzia?aj?ca.

Who changed what in which revision?

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