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