1 /** 2 * Copyright 2017 Google Inc. 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "pid.hpp" 18 19 #include "../tuning.hpp" 20 #include "logging.hpp" 21 22 namespace pid_control 23 { 24 namespace ec 25 { 26 27 /******************************** 28 * clamp 29 * 30 */ 31 static double clamp(double x, double min, double max) 32 { 33 if (x < min) 34 { 35 return min; 36 } 37 else if (x > max) 38 { 39 return max; 40 } 41 return x; 42 } 43 44 /******************************** 45 * pid code 46 * Note: Codes assumes the ts field is non-zero 47 */ 48 double pid(pid_info_t* pidinfoptr, double input, double setpoint, 49 const std::string* nameptr) 50 { 51 if (nameptr) 52 { 53 if (!(pidinfoptr->initialized)) 54 { 55 LogInit(*nameptr, pidinfoptr); 56 } 57 } 58 59 auto logPtr = nameptr ? LogPeek(*nameptr) : nullptr; 60 61 PidCoreContext coreContext; 62 std::chrono::milliseconds msNow; 63 64 if (logPtr) 65 { 66 msNow = LogTimestamp(); 67 } 68 69 coreContext.input = input; 70 coreContext.setpoint = setpoint; 71 72 double error; 73 74 double proportionalTerm; 75 double integralTerm = 0.0f; 76 double derivativeTerm = 0.0f; 77 double feedFwdTerm = 0.0f; 78 79 double output; 80 81 // calculate P, I, D, FF 82 83 // Pid 84 error = setpoint - input; 85 proportionalTerm = pidinfoptr->proportionalCoeff * error; 86 87 coreContext.error = error; 88 coreContext.proportionalTerm = proportionalTerm; 89 coreContext.integralTerm1 = 0.0; 90 91 // pId 92 if (0.0f != pidinfoptr->integralCoeff) 93 { 94 integralTerm = pidinfoptr->integral; 95 integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts; 96 97 coreContext.integralTerm1 = integralTerm; 98 99 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, 100 pidinfoptr->integralLimit.max); 101 } 102 103 coreContext.integralTerm2 = integralTerm; 104 105 // piD 106 derivativeTerm = pidinfoptr->derivativeCoeff * 107 ((error - pidinfoptr->lastError) / pidinfoptr->ts); 108 109 coreContext.derivativeTerm = derivativeTerm; 110 111 // FF 112 feedFwdTerm = (setpoint + pidinfoptr->feedFwdOffset) * 113 pidinfoptr->feedFwdGain; 114 115 coreContext.feedFwdTerm = feedFwdTerm; 116 117 output = proportionalTerm + integralTerm + derivativeTerm + feedFwdTerm; 118 119 coreContext.output1 = output; 120 121 output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max); 122 123 coreContext.output2 = output; 124 125 coreContext.minOut = 0.0; 126 coreContext.maxOut = 0.0; 127 128 // slew rate 129 // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic 130 // outLim_min/max that are affected by slew rate control and just clamping 131 // to those instead of effectively clamping twice. 132 if (pidinfoptr->initialized) 133 { 134 if (pidinfoptr->slewNeg != 0.0f) 135 { 136 // Don't decrease too fast 137 double minOut = pidinfoptr->lastOutput + 138 pidinfoptr->slewNeg * pidinfoptr->ts; 139 140 coreContext.minOut = minOut; 141 142 if (output < minOut) 143 { 144 output = minOut; 145 } 146 } 147 if (pidinfoptr->slewPos != 0.0f) 148 { 149 // Don't increase too fast 150 double maxOut = pidinfoptr->lastOutput + 151 pidinfoptr->slewPos * pidinfoptr->ts; 152 153 coreContext.maxOut = maxOut; 154 155 if (output > maxOut) 156 { 157 output = maxOut; 158 } 159 } 160 161 if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f) 162 { 163 // Back calculate integral term for the cases where we limited the 164 // output 165 integralTerm = output - proportionalTerm; 166 } 167 } 168 169 coreContext.output3 = output; 170 coreContext.integralTerm3 = integralTerm; 171 172 // Clamp again because having limited the output may result in a 173 // larger integral term 174 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, 175 pidinfoptr->integralLimit.max); 176 pidinfoptr->integral = integralTerm; 177 pidinfoptr->initialized = true; 178 pidinfoptr->lastError = error; 179 pidinfoptr->lastOutput = output; 180 181 coreContext.integralTerm = pidinfoptr->integral; 182 coreContext.output = pidinfoptr->lastOutput; 183 184 if (logPtr) 185 { 186 LogContext(*logPtr, msNow, coreContext); 187 } 188 189 return output; 190 } 191 192 } // namespace ec 193 } // namespace pid_control 194