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