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 
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 
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