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