David Spillman / PID

Fixed some mathematical issues

Dependents:   GPSNavigationNew

Fork of PID by Aaron Berk

PID.cpp@2:8a8bb3164e1c, 2015-04-05 (annotated)

Committer:
Spilly
Date:
Sun Apr 05 01:50:16 2015 +0000
Revision:
2:8a8bb3164e1c
Parent:
1:0861bf4f8f14
Child:
3:e3ef24ca1fd1
Fixed mathematical issue

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Spilly 2:8a8bb3164e1c 1 /**************************************************************************************************************************************************************
Spilly 2:8a8bb3164e1c 2 // This is a modified version of mbed /users/aberk/code/PID/ for LSM303DLHC
Spilly 2:8a8bb3164e1c 3 //
Spilly 2:8a8bb3164e1c 4 // Changes made by Ryan Spillman:
Spilly 2:8a8bb3164e1c 5 // Fixed a mathematical issue (need to better document said fix)
Spilly 2:8a8bb3164e1c 6 **************************************************************************************************************************************************************/
aberk 0:6e12a3e5af19 7 #include "PID.h"
aberk 0:6e12a3e5af19 8
Spilly 1:0861bf4f8f14 9 PID::PID(float Kc, float tauI, float tauD, float interval)
Spilly 1:0861bf4f8f14 10 {
aberk 0:6e12a3e5af19 11
aberk 0:6e12a3e5af19 12 usingFeedForward = false;
aberk 0:6e12a3e5af19 13 inAuto = false;
aberk 0:6e12a3e5af19 14
aberk 0:6e12a3e5af19 15 //Default the limits to the full range of I/O: 3.3V
aberk 0:6e12a3e5af19 16 //Make sure to set these to more appropriate limits for
aberk 0:6e12a3e5af19 17 //your application.
aberk 0:6e12a3e5af19 18 setInputLimits(0.0, 3.3);
aberk 0:6e12a3e5af19 19 setOutputLimits(0.0, 3.3);
aberk 0:6e12a3e5af19 20
aberk 0:6e12a3e5af19 21 tSample_ = interval;
aberk 0:6e12a3e5af19 22
aberk 0:6e12a3e5af19 23 setTunings(Kc, tauI, tauD);
aberk 0:6e12a3e5af19 24
aberk 0:6e12a3e5af19 25 setPoint_ = 0.0;
aberk 0:6e12a3e5af19 26 processVariable_ = 0.0;
aberk 0:6e12a3e5af19 27 prevProcessVariable_ = 0.0;
aberk 0:6e12a3e5af19 28 controllerOutput_ = 0.0;
aberk 0:6e12a3e5af19 29 prevControllerOutput_ = 0.0;
aberk 0:6e12a3e5af19 30
aberk 0:6e12a3e5af19 31 accError_ = 0.0;
aberk 0:6e12a3e5af19 32 bias_ = 0.0;
aberk 0:6e12a3e5af19 33
aberk 0:6e12a3e5af19 34 realOutput_ = 0.0;
aberk 0:6e12a3e5af19 35
aberk 0:6e12a3e5af19 36 }
aberk 0:6e12a3e5af19 37
Spilly 1:0861bf4f8f14 38 void PID::setInputLimits(float inMin, float inMax)
Spilly 1:0861bf4f8f14 39 {
aberk 0:6e12a3e5af19 40 inMin_ = inMin;
aberk 0:6e12a3e5af19 41 inMax_ = inMax;
aberk 0:6e12a3e5af19 42 inSpan_ = inMax - inMin;
aberk 0:6e12a3e5af19 43
aberk 0:6e12a3e5af19 44 }
aberk 0:6e12a3e5af19 45
Spilly 1:0861bf4f8f14 46 void PID::setOutputLimits(float outMin, float outMax)
Spilly 1:0861bf4f8f14 47 {
aberk 0:6e12a3e5af19 48
aberk 0:6e12a3e5af19 49 outMin_ = outMin;
aberk 0:6e12a3e5af19 50 outMax_ = outMax;
aberk 0:6e12a3e5af19 51 outSpan_ = outMax - outMin;
aberk 0:6e12a3e5af19 52
aberk 0:6e12a3e5af19 53 }
aberk 0:6e12a3e5af19 54
Spilly 1:0861bf4f8f14 55 void PID::setTunings(float Kc, float tauI, float tauD)
Spilly 1:0861bf4f8f14 56 {
aberk 0:6e12a3e5af19 57 //Store raw values to hand back to user on request.
aberk 0:6e12a3e5af19 58 pParam_ = Kc;
aberk 0:6e12a3e5af19 59 iParam_ = tauI;
aberk 0:6e12a3e5af19 60 dParam_ = tauD;
aberk 0:6e12a3e5af19 61
aberk 0:6e12a3e5af19 62 float tempTauR;
aberk 0:6e12a3e5af19 63
Spilly 1:0861bf4f8f14 64 if (tauI == 0.0f) {
Spilly 1:0861bf4f8f14 65 tempTauR = 0.0f;
aberk 0:6e12a3e5af19 66 } else {
Spilly 1:0861bf4f8f14 67 tempTauR = (1.0f / tauI) * tSample_;
aberk 0:6e12a3e5af19 68 }
aberk 0:6e12a3e5af19 69
aberk 0:6e12a3e5af19 70 //For "bumpless transfer" we need to rescale the accumulated error.
aberk 0:6e12a3e5af19 71 if (inAuto) {
Spilly 1:0861bf4f8f14 72 if (tempTauR == 0.0f) {
Spilly 1:0861bf4f8f14 73 accError_ = 0.0f;
aberk 0:6e12a3e5af19 74 } else {
aberk 0:6e12a3e5af19 75 accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
aberk 0:6e12a3e5af19 76 }
aberk 0:6e12a3e5af19 77 }
aberk 0:6e12a3e5af19 78
aberk 0:6e12a3e5af19 79 Kc_ = Kc;
aberk 0:6e12a3e5af19 80 tauR_ = tempTauR;
aberk 0:6e12a3e5af19 81 tauD_ = tauD / tSample_;
aberk 0:6e12a3e5af19 82
aberk 0:6e12a3e5af19 83 }
aberk 0:6e12a3e5af19 84
Spilly 1:0861bf4f8f14 85 void PID::reset(void)
Spilly 1:0861bf4f8f14 86 {
Spilly 1:0861bf4f8f14 87 prevControllerOutput_ = 0.0f;
aberk 0:6e12a3e5af19 88
aberk 0:6e12a3e5af19 89 //Clear any error in the integral.
Spilly 1:0861bf4f8f14 90 accError_ = 0.0f;
aberk 0:6e12a3e5af19 91
aberk 0:6e12a3e5af19 92 }
aberk 0:6e12a3e5af19 93
aberk 0:6e12a3e5af19 94 void PID::setMode(int mode) {
aberk 0:6e12a3e5af19 95
aberk 0:6e12a3e5af19 96 //We were in manual, and we just got set to auto.
aberk 0:6e12a3e5af19 97 //Reset the controller internals.
aberk 0:6e12a3e5af19 98 if (mode != 0 && !inAuto) {
aberk 0:6e12a3e5af19 99 reset();
aberk 0:6e12a3e5af19 100 }
aberk 0:6e12a3e5af19 101
aberk 0:6e12a3e5af19 102 inAuto = (mode != 0);
aberk 0:6e12a3e5af19 103
aberk 0:6e12a3e5af19 104 }
aberk 0:6e12a3e5af19 105
aberk 0:6e12a3e5af19 106 void PID::setInterval(float interval) {
aberk 0:6e12a3e5af19 107
aberk 0:6e12a3e5af19 108 if (interval > 0) {
aberk 0:6e12a3e5af19 109 //Convert the time-based tunings to reflect this change.
aberk 0:6e12a3e5af19 110 tauR_ *= (interval / tSample_);
aberk 0:6e12a3e5af19 111 accError_ *= (tSample_ / interval);
aberk 0:6e12a3e5af19 112 tauD_ *= (interval / tSample_);
aberk 0:6e12a3e5af19 113 tSample_ = interval;
aberk 0:6e12a3e5af19 114 }
aberk 0:6e12a3e5af19 115
aberk 0:6e12a3e5af19 116 }
aberk 0:6e12a3e5af19 117
aberk 0:6e12a3e5af19 118 void PID::setSetPoint(float sp) {
aberk 0:6e12a3e5af19 119
aberk 0:6e12a3e5af19 120 setPoint_ = sp;
aberk 0:6e12a3e5af19 121
aberk 0:6e12a3e5af19 122 }
aberk 0:6e12a3e5af19 123
aberk 0:6e12a3e5af19 124 void PID::setProcessValue(float pv) {
aberk 0:6e12a3e5af19 125
aberk 0:6e12a3e5af19 126 processVariable_ = pv;
aberk 0:6e12a3e5af19 127
aberk 0:6e12a3e5af19 128 }
aberk 0:6e12a3e5af19 129
aberk 0:6e12a3e5af19 130 void PID::setBias(float bias){
aberk 0:6e12a3e5af19 131
aberk 0:6e12a3e5af19 132 bias_ = bias;
aberk 0:6e12a3e5af19 133 usingFeedForward = 1;
aberk 0:6e12a3e5af19 134
aberk 0:6e12a3e5af19 135 }
aberk 0:6e12a3e5af19 136
Spilly 1:0861bf4f8f14 137 float PID::compute()
Spilly 1:0861bf4f8f14 138 {
Spilly 1:0861bf4f8f14 139 float error = setPoint_ - processVariable_;
aberk 0:6e12a3e5af19 140
aberk 0:6e12a3e5af19 141 //Check and see if the output is pegged at a limit and only
aberk 0:6e12a3e5af19 142 //integrate if it is not. This is to prevent reset-windup.
Spilly 1:0861bf4f8f14 143 if (!(prevControllerOutput_ >= outMax_ && error > 0) && !(prevControllerOutput_ <= outMin_ && error < 0)) {
aberk 0:6e12a3e5af19 144 accError_ += error;
aberk 0:6e12a3e5af19 145 }
aberk 0:6e12a3e5af19 146
aberk 0:6e12a3e5af19 147 //Compute the current slope of the input signal.
Spilly 1:0861bf4f8f14 148 float dMeas = (processVariable_ - prevProcessVariable_) / tSample_;
aberk 0:6e12a3e5af19 149
Spilly 1:0861bf4f8f14 150 float scaledBias = 0.0f;
aberk 0:6e12a3e5af19 151
aberk 0:6e12a3e5af19 152 //Perform the PID calculation.
aberk 0:6e12a3e5af19 153 controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
Spilly 1:0861bf4f8f14 154
Spilly 1:0861bf4f8f14 155 controllerOutput_ = ((outMax_ - outMin_) * ((Kc_ * controllerOutput_) - inMin_)) / (inMax_ - inMin_) + outMin_;
Spilly 1:0861bf4f8f14 156
aberk 0:6e12a3e5af19 157 //Make sure the computed output is within output constraints.
Spilly 1:0861bf4f8f14 158 if (controllerOutput_ < outMin_) {
Spilly 1:0861bf4f8f14 159 controllerOutput_ = outMin_;
Spilly 1:0861bf4f8f14 160 } else if (controllerOutput_ > outMax_) {
Spilly 1:0861bf4f8f14 161 controllerOutput_ = outMax_;
aberk 0:6e12a3e5af19 162 }
Spilly 1:0861bf4f8f14 163
aberk 0:6e12a3e5af19 164 //Remember this output for the windup check next time.
aberk 0:6e12a3e5af19 165 prevControllerOutput_ = controllerOutput_;
aberk 0:6e12a3e5af19 166 //Remember the input for the derivative calculation next time.
Spilly 1:0861bf4f8f14 167 prevProcessVariable_ = processVariable_;
Spilly 1:0861bf4f8f14 168
aberk 0:6e12a3e5af19 169 //Scale the output from percent span back out to a real world number.
Spilly 1:0861bf4f8f14 170 return controllerOutput_;
aberk 0:6e12a3e5af19 171 }
aberk 0:6e12a3e5af19 172
aberk 0:6e12a3e5af19 173 float PID::getInMin() {
aberk 0:6e12a3e5af19 174
aberk 0:6e12a3e5af19 175 return inMin_;
aberk 0:6e12a3e5af19 176
aberk 0:6e12a3e5af19 177 }
aberk 0:6e12a3e5af19 178
aberk 0:6e12a3e5af19 179 float PID::getInMax() {
aberk 0:6e12a3e5af19 180
aberk 0:6e12a3e5af19 181 return inMax_;
aberk 0:6e12a3e5af19 182
aberk 0:6e12a3e5af19 183 }
aberk 0:6e12a3e5af19 184
aberk 0:6e12a3e5af19 185 float PID::getOutMin() {
aberk 0:6e12a3e5af19 186
aberk 0:6e12a3e5af19 187 return outMin_;
aberk 0:6e12a3e5af19 188
aberk 0:6e12a3e5af19 189 }
aberk 0:6e12a3e5af19 190
aberk 0:6e12a3e5af19 191 float PID::getOutMax() {
aberk 0:6e12a3e5af19 192
aberk 0:6e12a3e5af19 193 return outMax_;
aberk 0:6e12a3e5af19 194
aberk 0:6e12a3e5af19 195 }
aberk 0:6e12a3e5af19 196
aberk 0:6e12a3e5af19 197 float PID::getInterval() {
aberk 0:6e12a3e5af19 198
aberk 0:6e12a3e5af19 199 return tSample_;
aberk 0:6e12a3e5af19 200
aberk 0:6e12a3e5af19 201 }
aberk 0:6e12a3e5af19 202
aberk 0:6e12a3e5af19 203 float PID::getPParam() {
aberk 0:6e12a3e5af19 204
aberk 0:6e12a3e5af19 205 return pParam_;
aberk 0:6e12a3e5af19 206
aberk 0:6e12a3e5af19 207 }
aberk 0:6e12a3e5af19 208
aberk 0:6e12a3e5af19 209 float PID::getIParam() {
aberk 0:6e12a3e5af19 210
aberk 0:6e12a3e5af19 211 return iParam_;
aberk 0:6e12a3e5af19 212
aberk 0:6e12a3e5af19 213 }
aberk 0:6e12a3e5af19 214
aberk 0:6e12a3e5af19 215 float PID::getDParam() {
aberk 0:6e12a3e5af19 216
aberk 0:6e12a3e5af19 217 return dParam_;
aberk 0:6e12a3e5af19 218
aberk 0:6e12a3e5af19 219 }