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 | |
Patrick Venture | a076487 | 2020-08-08 07:48:43 -0700 | [diff] [blame] | 19 | namespace pid_control |
| 20 | { |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 21 | namespace ec |
| 22 | { |
| 23 | |
| 24 | /******************************** |
| 25 | * clamp |
| 26 | * |
| 27 | */ |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 28 | static double clamp(double x, double min, double max) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 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 | */ |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 45 | double pid(pid_info_t* pidinfoptr, double input, double setpoint) |
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 error; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 48 | |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 49 | double proportionalTerm; |
| 50 | double integralTerm = 0.0f; |
Bonnie Lo | 0e8fc39 | 2022-10-05 10:20:55 +0800 | [diff] [blame] | 51 | double derivativeTerm = 0.0f; |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 52 | double feedFwdTerm = 0.0f; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 53 | |
Patrick Venture | 5f59c0f | 2018-11-11 12:55:14 -0800 | [diff] [blame] | 54 | double output; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 55 | |
| 56 | // calculate P, I, D, FF |
| 57 | |
| 58 | // Pid |
Patrick Venture | da4a5dd | 2018-08-31 09:42:48 -0700 | [diff] [blame] | 59 | error = setpoint - input; |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 60 | proportionalTerm = pidinfoptr->proportionalCoeff * error; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 61 | |
| 62 | // pId |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 63 | if (0.0f != pidinfoptr->integralCoeff) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 64 | { |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 65 | integralTerm = pidinfoptr->integral; |
| 66 | integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts; |
| 67 | integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, |
| 68 | pidinfoptr->integralLimit.max); |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 69 | } |
| 70 | |
Bonnie Lo | 0e8fc39 | 2022-10-05 10:20:55 +0800 | [diff] [blame] | 71 | // piD |
| 72 | derivativeTerm = pidinfoptr->derivativeCoeff * |
| 73 | ((error - pidinfoptr->lastError) / pidinfoptr->ts); |
| 74 | |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 75 | // FF |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 76 | feedFwdTerm = |
| 77 | (setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 78 | |
Bonnie Lo | 0e8fc39 | 2022-10-05 10:20:55 +0800 | [diff] [blame] | 79 | output = proportionalTerm + integralTerm + derivativeTerm + feedFwdTerm; |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 80 | output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max); |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 81 | |
| 82 | // slew rate |
| 83 | // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic |
Patrick Venture | 7442c37 | 2019-02-11 10:21:05 -0800 | [diff] [blame] | 84 | // outLim_min/max that are affected by slew rate control and just clamping |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 85 | // to those instead of effectively clamping twice. |
| 86 | if (pidinfoptr->initialized) |
| 87 | { |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 88 | if (pidinfoptr->slewNeg != 0.0f) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 89 | { |
| 90 | // Don't decrease too fast |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 91 | double minOut = |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 92 | pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts; |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 93 | if (output < minOut) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 94 | { |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 95 | output = minOut; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 96 | } |
| 97 | } |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 98 | if (pidinfoptr->slewPos != 0.0f) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 99 | { |
| 100 | // Don't increase too fast |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 101 | double maxOut = |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 102 | pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts; |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 103 | if (output > maxOut) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 104 | { |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 105 | output = maxOut; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 106 | } |
| 107 | } |
| 108 | |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 109 | if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f) |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 110 | { |
| 111 | // Back calculate integral term for the cases where we limited the |
| 112 | // output |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 113 | integralTerm = output - proportionalTerm; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 114 | } |
| 115 | } |
| 116 | |
| 117 | // Clamp again because having limited the output may result in a |
| 118 | // larger integral term |
Patrick Venture | a23468e | 2019-02-11 09:25:56 -0800 | [diff] [blame] | 119 | integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min, |
| 120 | pidinfoptr->integralLimit.max); |
| 121 | pidinfoptr->integral = integralTerm; |
Patrick Venture | da4a5dd | 2018-08-31 09:42:48 -0700 | [diff] [blame] | 122 | pidinfoptr->initialized = true; |
Bonnie Lo | 0e8fc39 | 2022-10-05 10:20:55 +0800 | [diff] [blame] | 123 | pidinfoptr->lastError = error; |
Patrick Venture | 4b0df32 | 2019-02-11 09:04:57 -0800 | [diff] [blame] | 124 | pidinfoptr->lastOutput = output; |
Patrick Venture | d801218 | 2018-03-08 08:21:38 -0800 | [diff] [blame] | 125 | |
| 126 | return output; |
| 127 | } |
| 128 | |
Patrick Venture | da4a5dd | 2018-08-31 09:42:48 -0700 | [diff] [blame] | 129 | } // namespace ec |
Patrick Venture | a076487 | 2020-08-08 07:48:43 -0700 | [diff] [blame] | 130 | } // namespace pid_control |