Francesco Pistone / Mbed 2 deprecated Viaro_SpandiConcime_V3_inizio

copia12092018

Dependencies:   mbed

PID/PID.cpp@13:4d6114864f2d, 2018-11-16 (annotated)

Committer:
root@developer-sjc-indigo-compiler.local.mbed.org
Date:
Fri Nov 16 10:55:45 2018 +0000
Revision:
13:4d6114864f2d
Parent:
0:b0a79a3a9da8 
Added tag fine for changeset dde73cf20353

Who changed what in which revision?

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