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
calPIDOutput(double setpt,double input,ec::pid_info_t * info)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
process(void)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