PIDController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::core::PIDController
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PIDController.h"
 
#include <memory>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
 
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
namespace armarx
{
    PIDController::PIDController(PIDController&& o) :
        Kp(o.Kp),
        Ki(o.Ki),
        Kd(o.Kd),
        integral(o.integral),
        maxIntegral(o.maxIntegral),
        conditionalIntegralErrorTreshold(o.conditionalIntegralErrorTreshold),
        derivative(o.derivative),
        previousError(o.previousError),
        processValue(o.processValue),
        target(o.target),
        lastUpdateTime(o.lastUpdateTime),
        controlValue(o.controlValue),
        controlValueDerivation(o.controlValueDerivation),
        maxControlValue(o.maxControlValue),
        maxDerivation(o.maxDerivation),
        firstRun(o.firstRun),
        limitless(o.limitless),
        threadSafe(o.threadSafe),
        differentialFilter(std::move(o.differentialFilter)),
        pdOutputFilter(std::move(o.pdOutputFilter))
    {
    }
 
    PIDController::PIDController(const PIDController& o) :
        Kp(o.Kp),
        Ki(o.Ki),
        Kd(o.Kd),
        integral(o.integral),
        maxIntegral(o.maxIntegral),
        conditionalIntegralErrorTreshold(o.conditionalIntegralErrorTreshold),
        derivative(o.derivative),
        previousError(o.previousError),
        processValue(o.processValue),
        target(o.target),
        lastUpdateTime(o.lastUpdateTime),
        controlValue(o.controlValue),
        controlValueDerivation(o.controlValueDerivation),
        maxControlValue(o.maxControlValue),
        maxDerivation(o.maxDerivation),
        firstRun(o.firstRun),
        limitless(o.limitless),
        threadSafe(o.threadSafe),
        differentialFilter(o.differentialFilter->clone()),
        pdOutputFilter(o.pdOutputFilter->clone())
    {
    }
 
    PIDController&
    PIDController::operator=(PIDController&& o)
    {
        Kp = o.Kp;
        Ki = o.Ki;
        Kd = o.Kd;
        integral = o.integral;
        maxIntegral = o.maxIntegral;
        conditionalIntegralErrorTreshold = o.conditionalIntegralErrorTreshold;
        derivative = o.derivative;
        previousError = o.previousError;
        processValue = o.processValue;
        target = o.target;
        lastUpdateTime = o.lastUpdateTime;
        controlValue = o.controlValue;
        controlValueDerivation = o.controlValueDerivation;
        maxControlValue = o.maxControlValue;
        maxDerivation = o.maxDerivation;
        firstRun = o.firstRun;
        limitless = o.limitless;
        threadSafe = o.threadSafe;
        differentialFilter = std::move(o.differentialFilter);
        pdOutputFilter = std::move(o.pdOutputFilter);
        return *this;
    }
 
    PIDController&
    PIDController::operator=(const PIDController& o)
    {
        Kp = o.Kp;
        Ki = o.Ki;
        Kd = o.Kd;
        integral = o.integral;
        maxIntegral = o.maxIntegral;
        conditionalIntegralErrorTreshold = o.conditionalIntegralErrorTreshold;
        derivative = o.derivative;
        previousError = o.previousError;
        processValue = o.processValue;
        target = o.target;
        lastUpdateTime = o.lastUpdateTime;
        controlValue = o.controlValue;
        controlValueDerivation = o.controlValueDerivation;
        maxControlValue = o.maxControlValue;
        maxDerivation = o.maxDerivation;
        firstRun = o.firstRun;
        limitless = o.limitless;
        threadSafe = o.threadSafe;
        differentialFilter = o.differentialFilter->clone();
        pdOutputFilter = o.pdOutputFilter->clone();
        return *this;
    }
 
    PIDController::PIDController(float Kp,
                                 float Ki,
                                 float Kd,
                                 double maxControlValue,
                                 double maxDerivation,
                                 bool limitless,
                                 bool threadSafe) :
        Kp(Kp),
        Ki(Ki),
        Kd(Kd),
        integral(0),
        derivative(0),
        previousError(0),
        processValue(0),
        target(0),
        controlValue(0),
        controlValueDerivation(0),
        maxControlValue(maxControlValue),
        maxDerivation(maxDerivation),
        limitless(limitless),
        threadSafe(threadSafe)
    {
        reset();
    }
 
    void
    PIDController::reset()
    {
        ScopedRecursiveLockPtr lock = getLock();
        firstRun = true;
        previousError = 0;
        integral = 0;
        lastUpdateTime = TimeUtil::GetTime();
        if (pdOutputFilter)
        {
            pdOutputFilter->reset();
        }
        if (differentialFilter)
        {
            differentialFilter->reset();
        }
    }
 
    auto
    PIDController::getLock() const -> ScopedRecursiveLockPtr
    {
        if (threadSafe)
        {
            return std::make_unique<ScopedRecursiveLock>(mutex);
        }
        else
        {
            return ScopedRecursiveLockPtr();
        }
    }
 
    void
    PIDController::update(double measuredValue, double targetValue)
    {
        ScopedRecursiveLockPtr lock = getLock();
        IceUtil::Time now = TimeUtil::GetTime();
 
        if (firstRun)
        {
            lastUpdateTime = TimeUtil::GetTime();
        }
 
        double dt = (now - lastUpdateTime).toSecondsDouble();
        update(dt, measuredValue, targetValue);
        lastUpdateTime = now;
    }
 
    void
    PIDController::update(double measuredValue)
    {
        ScopedRecursiveLockPtr lock = getLock();
        update(measuredValue, target);
    }
 
    void
    PIDController::setTarget(double newTarget)
    {
        ScopedRecursiveLockPtr lock = getLock();
        target = newTarget;
    }
 
    void
    PIDController::setMaxControlValue(double newMax)
    {
        ScopedRecursiveLockPtr lock = getLock();
        maxControlValue = newMax;
    }
 
    void
    PIDController::update(double deltaSec, double measuredValue, double targetValue)
    {
        ScopedRecursiveLockPtr lock = getLock();
        processValue = measuredValue;
        target = targetValue;
 
 
        double error = target - processValue;
        //ARMARX_INFO << VAROUT(target) << ", " << VAROUT(processValue) << ", " << VAROUT(error);
        if (limitless)
        {
            //ARMARX_INFO << VAROUT(error);
            error = math::MathUtils::angleModPI(error);
            //ARMARX_INFO << VAROUT(error);
            //error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
            //ARMARX_INFO << "Limitless after mod:" << VAROUT(error);
        }
 
        //double dt = (now - lastUpdateTime).toSecondsDouble();
        //    ARMARX_INFO << deactivateSpam() << VAROUT(dt);
        if (!firstRun)
        {
            if (std::abs(error) < conditionalIntegralErrorTreshold)
            {
                integral += error * deltaSec;
                integral = math::MathUtils::LimitTo(integral, maxIntegral);
            }
            if (deltaSec > 0.0)
            {
                derivative = (error - previousError) / deltaSec;
                if (differentialFilter)
                {
                    derivative = differentialFilter->update(deltaSec, derivative);
                }
            }
        }
 
        firstRun = false;
        double oldControlValue = controlValue;
        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;
            if (fabs(deriv) > maxDerivation)
            {
                controlValueDerivation = maxDerivation * deltaSec * math::MathUtils::Sign(deriv);
                controlValue = oldControlValue + controlValueDerivation;
            }
        }
 
        double const controlValueClamped =
            std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
 
        ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << ", kp*error: " << (Kp * error)
                     << ", cV: " << (controlValue) << ", max cV: " << (maxControlValue)
                     << ", cV clamped: " << controlValueClamped << ", i: " << (Ki * integral)
                     << ", d: " << (Kd * derivative) << ", dt: " << deltaSec;
 
        controlValue = controlValueClamped;
 
        previousError = error;
        lastUpdateTime += IceUtil::Time::secondsDouble(deltaSec);
    }
 
    double
    PIDController::getControlValue() const
    {
        ScopedRecursiveLockPtr lock = getLock();
        return controlValue;
    }
} // namespace armarx