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 "pidcontroller.hpp" 18 19 #include "ec/pid.hpp" 20 21 #include <algorithm> 22 #include <chrono> 23 #include <cmath> 24 #include <iostream> 25 #include <map> 26 #include <memory> 27 #include <vector> 28 29 namespace pid_control 30 { 31 32 double PIDController::calPIDOutput(double setpt, double input, 33 ec::pid_info_t* info) 34 { 35 double output; 36 auto name = getID(); 37 38 if (info->checkHysterWithSetpt) 39 { 40 // Over the hysteresis bounds, keep counting pid 41 if (input > (setpt + info->positiveHysteresis)) 42 { 43 // Calculate new output 44 output = ec::pid(info, input, setpt, &name); 45 46 // this variable isn't actually used in this context, but we're 47 // setting it here in case somebody uses it later it's the correct 48 // value 49 lastInput = input; 50 } 51 // Under the hysteresis bounds, initialize pid 52 else if (input < (setpt - info->negativeHysteresis)) 53 { 54 lastInput = setpt; 55 info->integral = 0; 56 output = 0; 57 } 58 // inside the hysteresis bounds, keep last output 59 else 60 { 61 lastInput = input; 62 output = info->lastOutput; 63 } 64 65 info->lastOutput = output; 66 } 67 else 68 { 69 // if no hysteresis, maintain previous behavior 70 if (info->positiveHysteresis == 0 && info->negativeHysteresis == 0) 71 { 72 // Calculate new output 73 output = ec::pid(info, input, setpt, &name); 74 75 // this variable isn't actually used in this context, but we're 76 // setting it here in case somebody uses it later it's the correct 77 // value 78 lastInput = input; 79 } 80 else 81 { 82 // initialize if the value is not set (NAN) or abnormal (+INF or 83 // -INF) 84 if (!(std::isfinite(lastInput))) 85 { 86 lastInput = input; 87 } 88 89 // if reading is outside of hysteresis bounds, use it for reading, 90 // otherwise use last reading without updating it first 91 else if ((input - lastInput) > info->positiveHysteresis) 92 { 93 lastInput = input; 94 } 95 else if ((lastInput - input) > info->negativeHysteresis) 96 { 97 lastInput = input; 98 } 99 100 output = ec::pid(info, lastInput, setpt, &name); 101 } 102 } 103 104 return output; 105 } 106 107 void PIDController::process(void) 108 { 109 double input; 110 double setpt; 111 double output; 112 113 // Get setpt value 114 setpt = setptProc(); 115 116 // Get input value 117 input = inputProc(); 118 119 auto info = getPIDInfo(); 120 121 // Calculate output value 122 output = calPIDOutput(setpt, input, info); 123 124 info->lastOutput = output; 125 126 // Output new value 127 outputProc(output); 128 129 return; 130 } 131 132 } // namespace pid_control 133