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