1 /*
2 * Copyright (C) 2016 Open Source Robotics Foundation
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
18 #include <chrono>
19 #include <cmath>
20 #include "ignition/math/Helpers.hh"
21 #include "ignition/math/PID.hh"
22
23 using namespace ignition;
24 using namespace math;
25
26 /////////////////////////////////////////////////
PID(const double _p,const double _i,const double _d,const double _imax,const double _imin,const double _cmdMax,const double _cmdMin,const double _cmdOffset)27 PID::PID(const double _p, const double _i, const double _d,
28 const double _imax, const double _imin, const double _cmdMax,
29 const double _cmdMin, const double _cmdOffset)
30 : pGain(_p), iGain(_i), dGain(_d), iMax(_imax), iMin(_imin),
31 cmdMax(_cmdMax), cmdMin(_cmdMin), cmdOffset(_cmdOffset)
32 {
33 this->Reset();
34 }
35
36 /////////////////////////////////////////////////
Init(const double _p,const double _i,const double _d,const double _imax,const double _imin,const double _cmdMax,const double _cmdMin,const double _cmdOffset)37 void PID::Init(const double _p, const double _i, const double _d,
38 const double _imax, const double _imin, const double _cmdMax,
39 const double _cmdMin, const double _cmdOffset)
40 {
41 this->pGain = _p;
42 this->iGain = _i;
43 this->dGain = _d;
44 this->iMax = _imax;
45 this->iMin = _imin;
46 this->cmdMax = _cmdMax;
47 this->cmdMin = _cmdMin;
48 this->cmdOffset = _cmdOffset;
49
50 this->Reset();
51 }
52
53 /////////////////////////////////////////////////
operator =(const PID & _p)54 PID &PID::operator=(const PID &_p)
55 {
56 if (this == &_p)
57 return *this;
58
59 this->pGain = _p.pGain;
60 this->iGain = _p.iGain;
61 this->dGain = _p.dGain;
62 this->iMax = _p.iMax;
63 this->iMin = _p.iMin;
64 this->cmdMax = _p.cmdMax;
65 this->cmdMin = _p.cmdMin;
66 this->cmdOffset = _p.cmdOffset;
67 this->pErrLast = _p.pErrLast;
68 this->pErr = _p.pErr;
69 this->iErr = _p.iErr;
70 this->dErr = _p.dErr;
71 this->cmd = _p.cmd;
72
73 return *this;
74 }
75
76 /////////////////////////////////////////////////
SetPGain(const double _p)77 void PID::SetPGain(const double _p)
78 {
79 this->pGain = _p;
80 }
81
82 /////////////////////////////////////////////////
SetIGain(const double _i)83 void PID::SetIGain(const double _i)
84 {
85 this->iGain = _i;
86 }
87
88 /////////////////////////////////////////////////
SetDGain(const double _d)89 void PID::SetDGain(const double _d)
90 {
91 this->dGain = _d;
92 }
93
94 /////////////////////////////////////////////////
SetIMax(const double _i)95 void PID::SetIMax(const double _i)
96 {
97 this->iMax = _i;
98 }
99
100 /////////////////////////////////////////////////
SetIMin(const double _i)101 void PID::SetIMin(const double _i)
102 {
103 this->iMin = _i;
104 }
105
106 /////////////////////////////////////////////////
SetCmdMax(const double _c)107 void PID::SetCmdMax(const double _c)
108 {
109 this->cmdMax = _c;
110 }
111
112 /////////////////////////////////////////////////
SetCmdMin(const double _c)113 void PID::SetCmdMin(const double _c)
114 {
115 this->cmdMin = _c;
116 }
117
118 /////////////////////////////////////////////////
SetCmdOffset(const double _c)119 void PID::SetCmdOffset(const double _c)
120 {
121 this->cmdOffset = _c;
122 }
123
124 /////////////////////////////////////////////////
Reset()125 void PID::Reset()
126 {
127 this->pErrLast = 0.0;
128 this->pErr = 0.0;
129 this->iErr = 0.0;
130 this->dErr = 0.0;
131 this->cmd = 0.0;
132 }
133
134 /////////////////////////////////////////////////
Update(const double _error,const std::chrono::duration<double> & _dt)135 double PID::Update(const double _error,
136 const std::chrono::duration<double> &_dt)
137 {
138 if (_dt == std::chrono::duration<double>(0) ||
139 ignition::math::isnan(_error) || std::isinf(_error))
140 {
141 return 0.0;
142 }
143
144 double pTerm, dTerm;
145 this->pErr = _error;
146
147 // Calculate proportional contribution to command
148 pTerm = this->pGain * this->pErr;
149
150 // Calculate the integral error
151 this->iErr = this->iErr + this->iGain * _dt.count() * this->pErr;
152
153 // Check the integral limits
154 // If enabled, this will limit iErr so that the limit is meaningful
155 // in the output
156 if (this->iMax >= this->iMin)
157 this->iErr = clamp(this->iErr, this->iMin, this->iMax);
158
159 // Calculate the derivative error
160 if (_dt != std::chrono::duration<double>(0))
161 {
162 this->dErr = (this->pErr - this->pErrLast) / _dt.count();
163 this->pErrLast = this->pErr;
164 }
165
166 // Calculate derivative contribution to command
167 dTerm = this->dGain * this->dErr;
168 this->cmd = this->cmdOffset -pTerm - this->iErr - dTerm;
169
170 // Check the command limits
171 if (this->cmdMax >= this->cmdMin)
172 this->cmd = clamp(this->cmd, this->cmdMin, this->cmdMax);
173
174 return this->cmd;
175 }
176
177 /////////////////////////////////////////////////
SetCmd(const double _cmd)178 void PID::SetCmd(const double _cmd)
179 {
180 this->cmd = _cmd;
181 }
182
183 /////////////////////////////////////////////////
Cmd() const184 double PID::Cmd() const
185 {
186 return this->cmd;
187 }
188
189 /////////////////////////////////////////////////
Errors(double & _pe,double & _ie,double & _de) const190 void PID::Errors(double &_pe, double &_ie, double &_de) const
191 {
192 _pe = this->pErr;
193 _ie = this->iErr;
194 _de = this->dErr;
195 }
196
197 /////////////////////////////////////////////////
PGain() const198 double PID::PGain() const
199 {
200 return this->pGain;
201 }
202
203 /////////////////////////////////////////////////
IGain() const204 double PID::IGain() const
205 {
206 return this->iGain;
207 }
208
209 /////////////////////////////////////////////////
DGain() const210 double PID::DGain() const
211 {
212 return this->dGain;
213 }
214
215 /////////////////////////////////////////////////
IMax() const216 double PID::IMax() const
217 {
218 return this->iMax;
219 }
220
221 /////////////////////////////////////////////////
IMin() const222 double PID::IMin() const
223 {
224 return this->iMin;
225 }
226
227 /////////////////////////////////////////////////
CmdMax() const228 double PID::CmdMax() const
229 {
230 return this->cmdMax;
231 }
232
233 /////////////////////////////////////////////////
CmdMin() const234 double PID::CmdMin() const
235 {
236 return this->cmdMin;
237 }
238
239 /////////////////////////////////////////////////
CmdOffset() const240 double PID::CmdOffset() const
241 {
242 return this->cmdOffset;
243 }
244