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 namespace pid_control 20 { 21 namespace ec 22 { 23 24 /******************************** 25 * clamp 26 * 27 */ 28 static double clamp(double x, double min, double max) 29 { 30 if (x < min) 31 { 32 return min; 33 } 34 else if (x > max) 35 { 36 return max; 37 } 38 return x; 39 } 40 41 /******************************** 42 * pid code 43 * Note: Codes assumes the ts field is non-zero 44 */ 45 double pid(pid_info_t* pidinfoptr, double input, double setpoint) 46 { 47 double error; 48 49 double proportionalTerm; 50 double integralTerm = 0.0f; 51 double derivativeTerm = 0.0f; 52 double feedFwdTerm = 0.0f; 53 54 double output; 55 56 // calculate P, I, D, FF 57 58 // Pid 59 error = setpoint - input; 60 proportionalTerm = pidinfoptr->proportionalCoeff * error; 61 62 // pId 63 if (0.0f != pidinfoptr->integralCoeff) 64 { 65 integralTerm = pidinfoptr->integral; 66 integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts; 67 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, 68 pidinfoptr->integralLimit.max); 69 } 70 71 // piD 72 derivativeTerm = pidinfoptr->derivativeCoeff * 73 ((error - pidinfoptr->lastError) / pidinfoptr->ts); 74 75 // FF 76 feedFwdTerm = 77 (setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain; 78 79 output = proportionalTerm + integralTerm + derivativeTerm + feedFwdTerm; 80 output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max); 81 82 // slew rate 83 // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic 84 // outLim_min/max that are affected by slew rate control and just clamping 85 // to those instead of effectively clamping twice. 86 if (pidinfoptr->initialized) 87 { 88 if (pidinfoptr->slewNeg != 0.0f) 89 { 90 // Don't decrease too fast 91 double minOut = 92 pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts; 93 if (output < minOut) 94 { 95 output = minOut; 96 } 97 } 98 if (pidinfoptr->slewPos != 0.0f) 99 { 100 // Don't increase too fast 101 double maxOut = 102 pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts; 103 if (output > maxOut) 104 { 105 output = maxOut; 106 } 107 } 108 109 if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f) 110 { 111 // Back calculate integral term for the cases where we limited the 112 // output 113 integralTerm = output - proportionalTerm; 114 } 115 } 116 117 // Clamp again because having limited the output may result in a 118 // larger integral term 119 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, 120 pidinfoptr->integralLimit.max); 121 pidinfoptr->integral = integralTerm; 122 pidinfoptr->initialized = true; 123 pidinfoptr->lastError = error; 124 pidinfoptr->lastOutput = output; 125 126 return output; 127 } 128 129 } // namespace ec 130 } // namespace pid_control 131