Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 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 | */ |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 26 | static double clamp(double x, double min, double max) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 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 | */ |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 43 | double pid(pid_info_t* pidinfoptr, double input, double setpoint) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 44 | { |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 45 | double error; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 46 | |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 47 | double p_term; |
| 48 | double i_term = 0.0f; |
| 49 | double ff_term = 0.0f; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 50 | |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 51 | double output; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 52 | |
| 53 | // calculate P, I, D, FF |
| 54 | |
| 55 | // Pid |
Patrick Venture | da4a5dd | 2018-08-31 09:42:48 -0700 | [diff] [blame] | 56 | error = setpoint - input; |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 57 | p_term = pidinfoptr->proportionalCoeff * error; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 58 | |
| 59 | // pId |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 60 | if (0.0f != pidinfoptr->integralCoeff) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 61 | { |
Patrick Venture | da4a5dd | 2018-08-31 09:42:48 -0700 | [diff] [blame] | 62 | i_term = pidinfoptr->integral; |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 63 | i_term += error * pidinfoptr->integralCoeff * pidinfoptr->ts; |
| 64 | i_term = clamp(i_term, pidinfoptr->integralLimit.min, |
| 65 | pidinfoptr->integralLimit.max); |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 66 | } |
| 67 | |
| 68 | // FF |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 69 | ff_term = (setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 70 | |
| 71 | output = p_term + i_term + ff_term; |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 72 | output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max); |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 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 | { |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 80 | if (pidinfoptr->slewNeg != 0.0f) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 81 | { |
| 82 | // Don't decrease too fast |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 83 | double min_out = |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 84 | pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 85 | if (output < min_out) |
| 86 | { |
| 87 | output = min_out; |
| 88 | } |
| 89 | } |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 90 | if (pidinfoptr->slewPos != 0.0f) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 91 | { |
| 92 | // Don't increase too fast |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 93 | double max_out = |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 94 | pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 95 | if (output > max_out) |
| 96 | { |
| 97 | output = max_out; |
| 98 | } |
| 99 | } |
| 100 | |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 101 | if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 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 |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 111 | i_term = clamp(i_term, pidinfoptr->integralLimit.min, |
| 112 | pidinfoptr->integralLimit.max); |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 113 | pidinfoptr->integral = i_term; |
Patrick Venture | da4a5dd | 2018-08-31 09:42:48 -0700 | [diff] [blame] | 114 | pidinfoptr->initialized = true; |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame^] | 115 | pidinfoptr->lastOutput = output; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 116 | |
| 117 | return output; |
| 118 | } |
| 119 | |
Patrick Venture | da4a5dd | 2018-08-31 09:42:48 -0700 | [diff] [blame] | 120 | } // namespace ec |