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