Skip to content
Snippets Groups Projects
Commit bf70f9b8 authored by Mirko Wächter's avatar Mirko Wächter
Browse files

added optional filter to pid controller

parent cea7fa9c
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
......@@ -68,6 +68,7 @@ namespace armarx
bool limitless;
bool threadSafe = true;
rtfilters::RTFilterBasePtr differentialFilter;
rtfilters::RTFilterBasePtr pdOutputFilter;
private:
ScopedRecursiveLockPtr getLock() const;
mutable RecursiveMutex mutex;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment