From bf70f9b8790b756b08535d0e1b8518a23e7a73ff Mon Sep 17 00:00:00 2001 From: Mirko Waechter <mirko.waechter@kit.edu> Date: Tue, 16 Apr 2019 11:19:36 +0200 Subject: [PATCH] added optional filter to pid controller --- source/RobotAPI/libraries/core/PIDController.cpp | 15 ++++++++++++++- source/RobotAPI/libraries/core/PIDController.h | 1 + 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp index fa980c0fa..69227baf8 100644 --- a/source/RobotAPI/libraries/core/PIDController.cpp +++ b/source/RobotAPI/libraries/core/PIDController.cpp @@ -57,6 +57,14 @@ void PIDController::reset() previousError = 0; integral = 0; lastUpdateTime = TimeUtil::GetTime(); + if (pdOutputFilter) + { + pdOutputFilter->reset(); + } + if (differentialFilter) + { + differentialFilter->reset(); + } } ScopedRecursiveLockPtr PIDController::getLock() const @@ -144,7 +152,12 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV firstRun = false; double oldControlValue = controlValue; - controlValue = Kp * error + Ki * integral + Kd * derivative; + double pdControlValue = Kp * error + Kd * derivative; + if (pdOutputFilter) + { + pdControlValue = pdOutputFilter->update(deltaSec, pdControlValue); + } + controlValue = pdControlValue + Ki * integral; if (deltaSec > 0.0) { double deriv = (controlValue - oldControlValue) / deltaSec; diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h index 164acfd90..5d9652e9a 100644 --- a/source/RobotAPI/libraries/core/PIDController.h +++ b/source/RobotAPI/libraries/core/PIDController.h @@ -68,6 +68,7 @@ namespace armarx bool limitless; bool threadSafe = true; rtfilters::RTFilterBasePtr differentialFilter; + rtfilters::RTFilterBasePtr pdOutputFilter; private: ScopedRecursiveLockPtr getLock() const; mutable RecursiveMutex mutex; -- GitLab