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