Fixed some mathematical issues
Fork of PID by
PID.cpp@4:b7326da8243b, 2015-04-27 (annotated)
- Committer:
- Spilly
- Date:
- Mon Apr 27 16:48:55 2015 +0000
- Revision:
- 4:b7326da8243b
- Parent:
- 3:e3ef24ca1fd1
- Child:
- 5:61d47c7736b5
BoatProject
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:e3ef24ca1fd1 | 6 | // |
Spilly | 3:e3ef24ca1fd1 | 7 | // Need to better document scaling and other mathematical issues |
Spilly | 2:8a8bb3164e1c | 8 | **************************************************************************************************************************************************************/ |
aberk | 0:6e12a3e5af19 | 9 | #include "PID.h" |
aberk | 0:6e12a3e5af19 | 10 | |
Spilly | 1:0861bf4f8f14 | 11 | PID::PID(float Kc, float tauI, float tauD, float interval) |
Spilly | 1:0861bf4f8f14 | 12 | { |
aberk | 0:6e12a3e5af19 | 13 | |
aberk | 0:6e12a3e5af19 | 14 | usingFeedForward = false; |
aberk | 0:6e12a3e5af19 | 15 | inAuto = false; |
aberk | 0:6e12a3e5af19 | 16 | |
aberk | 0:6e12a3e5af19 | 17 | //Default the limits to the full range of I/O: 3.3V |
aberk | 0:6e12a3e5af19 | 18 | //Make sure to set these to more appropriate limits for |
aberk | 0:6e12a3e5af19 | 19 | //your application. |
aberk | 0:6e12a3e5af19 | 20 | setInputLimits(0.0, 3.3); |
aberk | 0:6e12a3e5af19 | 21 | setOutputLimits(0.0, 3.3); |
aberk | 0:6e12a3e5af19 | 22 | |
aberk | 0:6e12a3e5af19 | 23 | tSample_ = interval; |
aberk | 0:6e12a3e5af19 | 24 | |
aberk | 0:6e12a3e5af19 | 25 | setTunings(Kc, tauI, tauD); |
aberk | 0:6e12a3e5af19 | 26 | |
aberk | 0:6e12a3e5af19 | 27 | setPoint_ = 0.0; |
aberk | 0:6e12a3e5af19 | 28 | processVariable_ = 0.0; |
aberk | 0:6e12a3e5af19 | 29 | prevProcessVariable_ = 0.0; |
aberk | 0:6e12a3e5af19 | 30 | controllerOutput_ = 0.0; |
aberk | 0:6e12a3e5af19 | 31 | prevControllerOutput_ = 0.0; |
aberk | 0:6e12a3e5af19 | 32 | |
aberk | 0:6e12a3e5af19 | 33 | accError_ = 0.0; |
aberk | 0:6e12a3e5af19 | 34 | bias_ = 0.0; |
aberk | 0:6e12a3e5af19 | 35 | |
aberk | 0:6e12a3e5af19 | 36 | realOutput_ = 0.0; |
aberk | 0:6e12a3e5af19 | 37 | |
aberk | 0:6e12a3e5af19 | 38 | } |
aberk | 0:6e12a3e5af19 | 39 | |
Spilly | 1:0861bf4f8f14 | 40 | void PID::setInputLimits(float inMin, float inMax) |
Spilly | 1:0861bf4f8f14 | 41 | { |
aberk | 0:6e12a3e5af19 | 42 | inMin_ = inMin; |
aberk | 0:6e12a3e5af19 | 43 | inMax_ = inMax; |
aberk | 0:6e12a3e5af19 | 44 | inSpan_ = inMax - inMin; |
aberk | 0:6e12a3e5af19 | 45 | |
aberk | 0:6e12a3e5af19 | 46 | } |
aberk | 0:6e12a3e5af19 | 47 | |
Spilly | 1:0861bf4f8f14 | 48 | void PID::setOutputLimits(float outMin, float outMax) |
Spilly | 1:0861bf4f8f14 | 49 | { |
aberk | 0:6e12a3e5af19 | 50 | |
aberk | 0:6e12a3e5af19 | 51 | outMin_ = outMin; |
aberk | 0:6e12a3e5af19 | 52 | outMax_ = outMax; |
aberk | 0:6e12a3e5af19 | 53 | outSpan_ = outMax - outMin; |
aberk | 0:6e12a3e5af19 | 54 | |
aberk | 0:6e12a3e5af19 | 55 | } |
aberk | 0:6e12a3e5af19 | 56 | |
Spilly | 1:0861bf4f8f14 | 57 | void PID::setTunings(float Kc, float tauI, float tauD) |
Spilly | 1:0861bf4f8f14 | 58 | { |
aberk | 0:6e12a3e5af19 | 59 | //Store raw values to hand back to user on request. |
aberk | 0:6e12a3e5af19 | 60 | pParam_ = Kc; |
aberk | 0:6e12a3e5af19 | 61 | iParam_ = tauI; |
aberk | 0:6e12a3e5af19 | 62 | dParam_ = tauD; |
aberk | 0:6e12a3e5af19 | 63 | |
aberk | 0:6e12a3e5af19 | 64 | float tempTauR; |
aberk | 0:6e12a3e5af19 | 65 | |
Spilly | 1:0861bf4f8f14 | 66 | if (tauI == 0.0f) { |
Spilly | 1:0861bf4f8f14 | 67 | tempTauR = 0.0f; |
aberk | 0:6e12a3e5af19 | 68 | } else { |
Spilly | 1:0861bf4f8f14 | 69 | tempTauR = (1.0f / tauI) * tSample_; |
aberk | 0:6e12a3e5af19 | 70 | } |
aberk | 0:6e12a3e5af19 | 71 | |
aberk | 0:6e12a3e5af19 | 72 | //For "bumpless transfer" we need to rescale the accumulated error. |
aberk | 0:6e12a3e5af19 | 73 | if (inAuto) { |
Spilly | 1:0861bf4f8f14 | 74 | if (tempTauR == 0.0f) { |
Spilly | 1:0861bf4f8f14 | 75 | accError_ = 0.0f; |
aberk | 0:6e12a3e5af19 | 76 | } else { |
aberk | 0:6e12a3e5af19 | 77 | accError_ *= (Kc_ * tauR_) / (Kc * tempTauR); |
aberk | 0:6e12a3e5af19 | 78 | } |
aberk | 0:6e12a3e5af19 | 79 | } |
aberk | 0:6e12a3e5af19 | 80 | |
aberk | 0:6e12a3e5af19 | 81 | Kc_ = Kc; |
aberk | 0:6e12a3e5af19 | 82 | tauR_ = tempTauR; |
aberk | 0:6e12a3e5af19 | 83 | tauD_ = tauD / tSample_; |
aberk | 0:6e12a3e5af19 | 84 | |
aberk | 0:6e12a3e5af19 | 85 | } |
aberk | 0:6e12a3e5af19 | 86 | |
Spilly | 1:0861bf4f8f14 | 87 | void PID::reset(void) |
Spilly | 1:0861bf4f8f14 | 88 | { |
Spilly | 1:0861bf4f8f14 | 89 | prevControllerOutput_ = 0.0f; |
aberk | 0:6e12a3e5af19 | 90 | |
aberk | 0:6e12a3e5af19 | 91 | //Clear any error in the integral. |
Spilly | 1:0861bf4f8f14 | 92 | accError_ = 0.0f; |
aberk | 0:6e12a3e5af19 | 93 | |
aberk | 0:6e12a3e5af19 | 94 | } |
aberk | 0:6e12a3e5af19 | 95 | |
aberk | 0:6e12a3e5af19 | 96 | void PID::setMode(int mode) { |
aberk | 0:6e12a3e5af19 | 97 | |
aberk | 0:6e12a3e5af19 | 98 | //We were in manual, and we just got set to auto. |
aberk | 0:6e12a3e5af19 | 99 | //Reset the controller internals. |
aberk | 0:6e12a3e5af19 | 100 | if (mode != 0 && !inAuto) { |
aberk | 0:6e12a3e5af19 | 101 | reset(); |
aberk | 0:6e12a3e5af19 | 102 | } |
aberk | 0:6e12a3e5af19 | 103 | |
aberk | 0:6e12a3e5af19 | 104 | inAuto = (mode != 0); |
aberk | 0:6e12a3e5af19 | 105 | |
aberk | 0:6e12a3e5af19 | 106 | } |
aberk | 0:6e12a3e5af19 | 107 | |
aberk | 0:6e12a3e5af19 | 108 | void PID::setInterval(float interval) { |
aberk | 0:6e12a3e5af19 | 109 | |
aberk | 0:6e12a3e5af19 | 110 | if (interval > 0) { |
aberk | 0:6e12a3e5af19 | 111 | //Convert the time-based tunings to reflect this change. |
aberk | 0:6e12a3e5af19 | 112 | tauR_ *= (interval / tSample_); |
aberk | 0:6e12a3e5af19 | 113 | accError_ *= (tSample_ / interval); |
aberk | 0:6e12a3e5af19 | 114 | tauD_ *= (interval / tSample_); |
aberk | 0:6e12a3e5af19 | 115 | tSample_ = interval; |
aberk | 0:6e12a3e5af19 | 116 | } |
aberk | 0:6e12a3e5af19 | 117 | |
aberk | 0:6e12a3e5af19 | 118 | } |
aberk | 0:6e12a3e5af19 | 119 | |
aberk | 0:6e12a3e5af19 | 120 | void PID::setSetPoint(float sp) { |
aberk | 0:6e12a3e5af19 | 121 | |
aberk | 0:6e12a3e5af19 | 122 | setPoint_ = sp; |
aberk | 0:6e12a3e5af19 | 123 | |
aberk | 0:6e12a3e5af19 | 124 | } |
aberk | 0:6e12a3e5af19 | 125 | |
aberk | 0:6e12a3e5af19 | 126 | void PID::setProcessValue(float pv) { |
aberk | 0:6e12a3e5af19 | 127 | |
aberk | 0:6e12a3e5af19 | 128 | processVariable_ = pv; |
aberk | 0:6e12a3e5af19 | 129 | |
aberk | 0:6e12a3e5af19 | 130 | } |
aberk | 0:6e12a3e5af19 | 131 | |
aberk | 0:6e12a3e5af19 | 132 | void PID::setBias(float bias){ |
aberk | 0:6e12a3e5af19 | 133 | |
aberk | 0:6e12a3e5af19 | 134 | bias_ = bias; |
aberk | 0:6e12a3e5af19 | 135 | usingFeedForward = 1; |
aberk | 0:6e12a3e5af19 | 136 | |
aberk | 0:6e12a3e5af19 | 137 | } |
aberk | 0:6e12a3e5af19 | 138 | |
Spilly | 1:0861bf4f8f14 | 139 | float PID::compute() |
Spilly | 1:0861bf4f8f14 | 140 | { |
Spilly | 1:0861bf4f8f14 | 141 | float error = setPoint_ - processVariable_; |
aberk | 0:6e12a3e5af19 | 142 | |
aberk | 0:6e12a3e5af19 | 143 | //Check and see if the output is pegged at a limit and only |
aberk | 0:6e12a3e5af19 | 144 | //integrate if it is not. This is to prevent reset-windup. |
Spilly | 1:0861bf4f8f14 | 145 | if (!(prevControllerOutput_ >= outMax_ && error > 0) && !(prevControllerOutput_ <= outMin_ && error < 0)) { |
aberk | 0:6e12a3e5af19 | 146 | accError_ += error; |
aberk | 0:6e12a3e5af19 | 147 | } |
aberk | 0:6e12a3e5af19 | 148 | |
aberk | 0:6e12a3e5af19 | 149 | //Compute the current slope of the input signal. |
Spilly | 1:0861bf4f8f14 | 150 | float dMeas = (processVariable_ - prevProcessVariable_) / tSample_; |
aberk | 0:6e12a3e5af19 | 151 | |
aberk | 0:6e12a3e5af19 | 152 | //Perform the PID calculation. |
Spilly | 4:b7326da8243b | 153 | controllerOutput_ = 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 | } |