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