PIDController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotAPI::core::PIDController
19  * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "PIDController.h"
31 
32 #include <memory>
33 
34 namespace armarx
35 {
37  Kp(o.Kp),
38  Ki(o.Ki),
39  Kd(o.Kd),
40  integral(o.integral),
41  maxIntegral(o.maxIntegral),
42  conditionalIntegralErrorTreshold(o.conditionalIntegralErrorTreshold),
43  derivative(o.derivative),
44  previousError(o.previousError),
45  processValue(o.processValue),
46  target(o.target),
47  lastUpdateTime(o.lastUpdateTime),
48  controlValue(o.controlValue),
49  controlValueDerivation(o.controlValueDerivation),
50  maxControlValue(o.maxControlValue),
51  maxDerivation(o.maxDerivation),
52  firstRun(o.firstRun),
53  limitless(o.limitless),
54  threadSafe(o.threadSafe),
55  differentialFilter(std::move(o.differentialFilter)),
56  pdOutputFilter(std::move(o.pdOutputFilter))
57  {}
59  Kp(o.Kp),
60  Ki(o.Ki),
61  Kd(o.Kd),
62  integral(o.integral),
63  maxIntegral(o.maxIntegral),
64  conditionalIntegralErrorTreshold(o.conditionalIntegralErrorTreshold),
65  derivative(o.derivative),
66  previousError(o.previousError),
67  processValue(o.processValue),
68  target(o.target),
69  lastUpdateTime(o.lastUpdateTime),
70  controlValue(o.controlValue),
71  controlValueDerivation(o.controlValueDerivation),
72  maxControlValue(o.maxControlValue),
73  maxDerivation(o.maxDerivation),
74  firstRun(o.firstRun),
75  limitless(o.limitless),
76  threadSafe(o.threadSafe),
77  differentialFilter(o.differentialFilter->clone()),
78  pdOutputFilter(o.pdOutputFilter->clone())
79  {}
81  {
82  Kp = o.Kp;
83  Ki = o.Ki;
84  Kd = o.Kd;
85  integral = o.integral;
86  maxIntegral = o.maxIntegral;
87  conditionalIntegralErrorTreshold = o.conditionalIntegralErrorTreshold;
88  derivative = o.derivative;
89  previousError = o.previousError;
90  processValue = o.processValue;
91  target = o.target;
92  lastUpdateTime = o.lastUpdateTime;
93  controlValue = o.controlValue;
94  controlValueDerivation = o.controlValueDerivation;
95  maxControlValue = o.maxControlValue;
96  maxDerivation = o.maxDerivation;
97  firstRun = o.firstRun;
98  limitless = o.limitless;
99  threadSafe = o.threadSafe;
100  differentialFilter = std::move(o.differentialFilter);
101  pdOutputFilter = std::move(o.pdOutputFilter);
102  return *this;
103  }
105  {
106  Kp = o.Kp;
107  Ki = o.Ki;
108  Kd = o.Kd;
109  integral = o.integral;
115  target = o.target;
121  firstRun = o.firstRun;
122  limitless = o.limitless;
125  pdOutputFilter = o.pdOutputFilter->clone();
126  return *this;
127  }
128 
129  PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, bool threadSafe) :
130  Kp(Kp),
131  Ki(Ki),
132  Kd(Kd),
133  integral(0),
134  derivative(0),
135  previousError(0),
136  processValue(0),
137  target(0),
138  controlValue(0),
139  controlValueDerivation(0),
140  maxControlValue(maxControlValue),
141  maxDerivation(maxDerivation),
142  limitless(limitless),
143  threadSafe(threadSafe)
144  {
145  reset();
146  }
147 
149  {
150  ScopedRecursiveLockPtr lock = getLock();
151  firstRun = true;
152  previousError = 0;
153  integral = 0;
155  if (pdOutputFilter)
156  {
157  pdOutputFilter->reset();
158  }
159  if (differentialFilter)
160  {
161  differentialFilter->reset();
162  }
163  }
164 
165  auto PIDController::getLock() const -> ScopedRecursiveLockPtr
166  {
167  if (threadSafe)
168  {
169  return std::make_unique<ScopedRecursiveLock>(mutex);
170  }
171  else
172  {
173  return ScopedRecursiveLockPtr();
174  }
175  }
176 
177  void PIDController::update(double measuredValue, double targetValue)
178  {
179  ScopedRecursiveLockPtr lock = getLock();
181 
182  if (firstRun)
183  {
185  }
186 
187  double dt = (now - lastUpdateTime).toSecondsDouble();
188  update(dt, measuredValue, targetValue);
189  lastUpdateTime = now;
190  }
191 
192 
193  void PIDController::update(double measuredValue)
194  {
195  ScopedRecursiveLockPtr lock = getLock();
196  update(measuredValue, target);
197  }
198 
199  void PIDController::setTarget(double newTarget)
200  {
201  ScopedRecursiveLockPtr lock = getLock();
202  target = newTarget;
203  }
204 
206  {
207  ScopedRecursiveLockPtr lock = getLock();
208  maxControlValue = newMax;
209  }
210 
211  void PIDController::update(double deltaSec, double measuredValue, double targetValue)
212  {
213  ScopedRecursiveLockPtr lock = getLock();
214  processValue = measuredValue;
215  target = targetValue;
216 
217 
218  double error = target - processValue;
219  //ARMARX_INFO << VAROUT(target) << ", " << VAROUT(processValue) << ", " << VAROUT(error);
220  if (limitless)
221  {
222  //ARMARX_INFO << VAROUT(error);
223  error = math::MathUtils::angleModPI(error);
224  //ARMARX_INFO << VAROUT(error);
225  //error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
226  //ARMARX_INFO << "Limitless after mod:" << VAROUT(error);
227  }
228 
229  //double dt = (now - lastUpdateTime).toSecondsDouble();
230  // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
231  if (!firstRun)
232  {
234  {
235  integral += error * deltaSec;
237  }
238  if (deltaSec > 0.0)
239  {
240  derivative = (error - previousError) / deltaSec;
241  if (differentialFilter)
242  {
243  derivative = differentialFilter->update(deltaSec, derivative);
244  }
245  }
246  }
247 
248  firstRun = false;
249  double oldControlValue = controlValue;
250  double pdControlValue = Kp * error + Kd * derivative;
251  if (pdOutputFilter)
252  {
253  pdControlValue = pdOutputFilter->update(deltaSec, pdControlValue);
254  }
255  controlValue = pdControlValue + Ki * integral;
256  if (deltaSec > 0.0)
257  {
258  double deriv = (controlValue - oldControlValue) / deltaSec;
259  if (fabs(deriv) > maxDerivation)
260  {
262  controlValue = oldControlValue + controlValueDerivation;
263  }
264  }
266  ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
267 
268  previousError = error;
269  lastUpdateTime += IceUtil::Time::secondsDouble(deltaSec);
270 
271  }
272 
274  {
275  ScopedRecursiveLockPtr lock = getLock();
276  return controlValue;
277  }
278 }
279 
280 
281 
282 
283 
284 
285 
286 
287 
288 
289 
armarx::PIDController::maxControlValue
double maxControlValue
Definition: PIDController.h:79
armarx::PIDController::processValue
double processValue
Definition: PIDController.h:74
armarx::PIDController::conditionalIntegralErrorTreshold
double conditionalIntegralErrorTreshold
Definition: PIDController.h:71
MathUtils.h
armarx::PIDController::Kp
float Kp
Definition: PIDController.h:68
armarx::PIDController::operator=
PIDController & operator=(PIDController &&o)
Definition: PIDController.cpp:80
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
armarx::PIDController::Ki
float Ki
Definition: PIDController.h:68
armarx::PIDController::Kd
float Kd
Definition: PIDController.h:68
armarx::PIDController::getControlValue
double getControlValue() const
Definition: PIDController.cpp:273
armarx::PIDController::setTarget
void setTarget(double newTarget)
Definition: PIDController.cpp:199
armarx::PIDController::update
void update(double deltaSec, double measuredValue, double targetValue)
Definition: PIDController.cpp:211
armarx::PIDController::reset
void reset()
Definition: PIDController.cpp:148
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
armarx::PIDController::PIDController
PIDController()=default
ButterworthFilter.h
armarx::PIDController
Definition: PIDController.h:43
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::PIDController::limitless
bool limitless
Definition: PIDController.h:82
armarx::PIDController::previousError
double previousError
Definition: PIDController.h:73
armarx::math::MathUtils::angleModPI
static float angleModPI(float value)
Definition: MathUtils.h:140
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::PIDController::controlValueDerivation
double controlValueDerivation
Definition: PIDController.h:78
armarx::PIDController::threadSafe
bool threadSafe
Definition: PIDController.h:83
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::PIDController::integral
double integral
Definition: PIDController.h:69
ExpressionException.h
PIDController.h
armarx::PIDController::controlValue
double controlValue
Definition: PIDController.h:77
armarx::math::MathUtils::LimitTo
static double LimitTo(double value, double absThreshold)
Definition: MathUtils.h:56
armarx::PIDController::maxIntegral
double maxIntegral
Definition: PIDController.h:70
TimeUtil.h
std
Definition: Application.h:66
armarx::math::MathUtils::Sign
static int Sign(double value)
Definition: MathUtils.h:34
RTFilterBase.h
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:92
armarx::PIDController::target
double target
Definition: PIDController.h:75
armarx::PIDController::derivative
double derivative
Definition: PIDController.h:72
min
T min(T t1, T t2)
Definition: gdiam.h:42
armarx::PIDController::lastUpdateTime
IceUtil::Time lastUpdateTime
Definition: PIDController.h:76
armarx::ScopedRecursiveLockPtr
std::shared_ptr< ScopedRecursiveLock > ScopedRecursiveLockPtr
Definition: Synchronization.h:143
armarx::PIDController::maxDerivation
double maxDerivation
Definition: PIDController.h:80
armarx::PIDController::pdOutputFilter
std::shared_ptr< rtfilters::RTFilterBase > pdOutputFilter
Definition: PIDController.h:85
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::PIDController::firstRun
bool firstRun
Definition: PIDController.h:81
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:42
armarx::PIDController::differentialFilter
std::shared_ptr< rtfilters::RTFilterBase > differentialFilter
Definition: PIDController.h:84
armarx::PIDController::setMaxControlValue
void setMaxControlValue(double newMax)
Definition: PIDController.cpp:205