PidController.hpp

[詳解]

//
//    PIDコントローラクラス
//
//    Copyright (c) 2016-2023 谷川 豊章
//    Copyright (c) 2016-2023 udonrobo
//
 
#ifndef DEF_PidController_H
#define DEF_PidController_H
 
#include "Math.hpp"
#include <Udon/Types/Optional.hpp>
#include <Udon/Types/Range.hpp>
 
namespace Udon
{

    class PidController
    {
    public:
        struct Parameter
        {
            double p;
            double i;
            double d;
        };
 
    private:
        Parameter constant;    
 
        Udon::Optional<Parameter> requestConstant;    
 
        Parameter power;    
 
        double lastError;    
 
        const double MAX_INT_POWER;    
        const double INTERVAL_S;       
 
    public:
        PidController(double kPro, double kInt, double kDif, unsigned long callInterval_us, double maxIntPower = 1023.0) noexcept
            : constant{ kPro, kInt, kDif }
            , requestConstant{}
            , power{}
            , lastError()
            , MAX_INT_POWER(maxIntPower)
            , INTERVAL_S(callInterval_us / 1000000.0)
        {
        }

        void update(double controlValue, double targetValue) noexcept
        {
            // 係数の適用
            const auto coefficient = requestConstant ? *requestConstant : constant;
            requestConstant.reset();
 
            // 偏差の計算
            const double error = targetValue - controlValue;
 
            // 比例量の計算
            power.p = coefficient.p * error;
 
            // 積分量の計算
            power.i += coefficient.i * error * INTERVAL_S;
            power.i = Udon::Constrain(power.i, -MAX_INT_POWER, MAX_INT_POWER);
 
            // 微分量の計算
            power.d = coefficient.d * (error - lastError) / INTERVAL_S;
 
            // 偏差の保存
            lastError = error;
        }

        double getPower() const noexcept
        {
            return power.p + power.i + power.d;
        }

        double getPower(double min, double max) const noexcept
        {
            return Udon::Constrain(getPower(), min, max);
        }

        double getPower(const Udon::Range<double>& range) const noexcept
        {
            return getPower(range.min, range.max);
        }

        double operator()(double controlValue, double targetValue) noexcept
        {
            update(controlValue, targetValue);
            return getPower();
        }

        double operator()(double controlValue, double targetValue, double min, double max) noexcept
        {
            update(controlValue, targetValue);
            return getPower(min, max);
        }

        double operator()(double controlValue, double targetValue, const Udon::Range<double>& range) noexcept
        {
            return operator()(controlValue, targetValue, range.min, range.max);
        }

        void clearPower() noexcept
        {
            power = {};
            lastError = 0.0;
        }
        void clearIntegral() noexcept
        {
            power.i = 0.0;
        }

        void requestParamPro(double value) noexcept
        {
            if (requestConstant)
            {
                requestConstant->p = value;
            }
            else
            {
                requestConstant = Parameter{ value, constant.i, constant.d };
            }
        }

        void requestParamInt(double value) noexcept
        {
            if (requestConstant)
            {
                requestConstant->i = value;
            }
            else
            {
                requestConstant = Parameter{ constant.p, value, constant.d };
            }
        }

        void requestParamDif(double value) noexcept
        {
            if (requestConstant)
            {
                requestConstant->d = value;
            }
            else
            {
                requestConstant = Parameter{ constant.p, constant.i, value };
            }
        }

        void requestParam(const Parameter& value) noexcept { requestConstant = value; }

        void setParamPro(double value) noexcept { constant.p = value; }

        void setParamInt(double value) noexcept { constant.i = value; }

        void setParamDif(double value) noexcept { constant.d = value; }

        void setParam(const Parameter& value) noexcept { constant = value; }

        double getParamPro() const noexcept { return constant.p; }

        double getParamInt() const noexcept { return constant.i; }

        double getParamDif() const noexcept { return constant.d; }

        const Parameter& getParam() const noexcept { return constant; }

        double getPowerPro() const noexcept { return power.p; }

        double getPowerInt() const noexcept { return power.i; }

        double getPowerDif() const noexcept { return power.d; }
    };
 
}    // namespace Udon
 
#endif