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 void PIDController::process(void)
34 {
35     double input;
36     double setpt;
37     double output;
38 
39     // Get setpt value
40     setpt = setptProc();
41 
42     // Get input value
43     input = inputProc();
44 
45     auto info = getPIDInfo();
46     auto name = getID();
47 
48     // if no hysteresis, maintain previous behavior
49     if (info->positiveHysteresis == 0 && info->negativeHysteresis == 0)
50     {
51         // Calculate new output
52         output = ec::pid(info, input, setpt, &name);
53 
54         // this variable isn't actually used in this context, but we're setting
55         // it here incase somebody uses it later it's the correct value
56         lastInput = input;
57     }
58     else
59     {
60         // initialize if not set yet
61         if (std::isnan(lastInput))
62         {
63             lastInput = input;
64         }
65 
66         // if reading is outside of hysteresis bounds, use it for reading,
67         // otherwise use last reading without updating it first
68         else if ((input - lastInput) > info->positiveHysteresis)
69         {
70             lastInput = input;
71         }
72         else if ((lastInput - input) > info->negativeHysteresis)
73         {
74             lastInput = input;
75         }
76 
77         output = ec::pid(info, lastInput, setpt, &name);
78     }
79 
80     // Output new value
81     outputProc(output);
82 
83     return;
84 }
85 
86 } // namespace pid_control
87