1+ /* *
2+ * Copyright 2019 Bradley J. Snyder <snyder.bradleyj@gmail.com>
3+ *
4+ * Permission is hereby granted, free of charge, to any person obtaining a copy
5+ * of this software and associated documentation files (the "Software"), to deal
6+ * in the Software without restriction, including without limitation the rights
7+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8+ * copies of the Software, and to permit persons to whom the Software is
9+ * furnished to do so, subject to the following conditions:
10+ *
11+ * The above copyright notice and this permission notice shall be included in
12+ * all copies or substantial portions of the Software.
13+ *
14+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20+ * THE SOFTWARE.
21+ */
22+
23+ #ifndef _PID_SOURCE_
24+ #define _PID_SOURCE_
25+
26+ #include < iostream>
27+ #include < cmath>
28+ #include " pid.h"
29+
30+ using namespace std ;
31+
32+ class PIDImpl
33+ {
34+ public:
35+ PIDImpl ( double dt, double max, double min, double Kp, double Kd, double Ki );
36+ ~PIDImpl ();
37+ double calculate ( double setpoint, double pv );
38+
39+ private:
40+ double _dt;
41+ double _max;
42+ double _min;
43+ double _Kp;
44+ double _Kd;
45+ double _Ki;
46+ double _pre_error;
47+ double _integral;
48+ };
49+
50+
51+ PID::PID ( double dt, double max, double min, double Kp, double Kd, double Ki )
52+ {
53+ pimpl = new PIDImpl (dt,max,min,Kp,Kd,Ki);
54+ }
55+ double PID::calculate ( double setpoint, double pv )
56+ {
57+ return pimpl->calculate (setpoint,pv);
58+ }
59+ PID::~PID ()
60+ {
61+ delete pimpl;
62+ }
63+
64+ /* *
65+ * Implementation
66+ */
67+ PIDImpl::PIDImpl ( double dt, double max, double min, double Kp, double Kd, double Ki ) :
68+ _dt(dt),
69+ _max(max),
70+ _min(min),
71+ _Kp(Kp),
72+ _Kd(Kd),
73+ _Ki(Ki),
74+ _pre_error(0 ),
75+ _integral(0 )
76+ {
77+ }
78+
79+ double PIDImpl::calculate ( double setpoint, double pv )
80+ {
81+
82+ // Calculate error
83+ double error = setpoint - pv;
84+
85+ // Proportional term
86+ double Pout = _Kp * error;
87+
88+ // Integral term
89+ _integral += error * _dt;
90+ double Iout = _Ki * _integral;
91+
92+ // Derivative term
93+ double derivative = (error - _pre_error) / _dt;
94+ double Dout = _Kd * derivative;
95+
96+ // Calculate total output
97+ double output = Pout + Iout + Dout;
98+
99+ // Restrict to max/min
100+ if ( output > _max )
101+ output = _max;
102+ else if ( output < _min )
103+ output = _min;
104+
105+ // Save error to previous error
106+ _pre_error = error;
107+
108+ return output;
109+ }
110+
111+ PIDImpl::~PIDImpl ()
112+ {
113+ }
114+
115+ #endif
0 commit comments