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 feedFwdTerm = 0.0f; 52 53 double output; 54 55 // calculate P, I, D, FF 56 57 // Pid 58 error = setpoint - input; 59 proportionalTerm = pidinfoptr->proportionalCoeff * error; 60 61 // pId 62 if (0.0f != pidinfoptr->integralCoeff) 63 { 64 integralTerm = pidinfoptr->integral; 65 integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts; 66 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, 67 pidinfoptr->integralLimit.max); 68 } 69 70 // FF 71 feedFwdTerm = 72 (setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain; 73 74 output = proportionalTerm + integralTerm + feedFwdTerm; 75 output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max); 76 77 // slew rate 78 // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic 79 // outLim_min/max that are affected by slew rate control and just clamping 80 // to those instead of effectively clamping twice. 81 if (pidinfoptr->initialized) 82 { 83 if (pidinfoptr->slewNeg != 0.0f) 84 { 85 // Don't decrease too fast 86 double minOut = 87 pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts; 88 if (output < minOut) 89 { 90 output = minOut; 91 } 92 } 93 if (pidinfoptr->slewPos != 0.0f) 94 { 95 // Don't increase too fast 96 double maxOut = 97 pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts; 98 if (output > maxOut) 99 { 100 output = maxOut; 101 } 102 } 103 104 if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f) 105 { 106 // Back calculate integral term for the cases where we limited the 107 // output 108 integralTerm = output - proportionalTerm; 109 } 110 } 111 112 // Clamp again because having limited the output may result in a 113 // larger integral term 114 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, 115 pidinfoptr->integralLimit.max); 116 pidinfoptr->integral = integralTerm; 117 pidinfoptr->initialized = true; 118 pidinfoptr->lastOutput = output; 119 120 return output; 121 } 122 123 } // namespace ec 124 } // namespace pid_control 125