SpeedPid.hpp

[詳解]

//
//    速度型PIDコントローラクラス
//
 
#ifndef DEF_SpeedPidController_H
#define DEF_SpeedPidController_H
 
#include "Math.hpp"
#include <Udon/Types/Optional.hpp>
#include <Udon/Types/Range.hpp>
 
namespace Udon
{
    class SpeedPidController
    {
    public:
        struct Parameter
        {
            double p;
            double i;
            double d;
        };
 
    private:
        Parameter constant;    
 
        Udon::Optional<Parameter> requestConstant;    
 
        Parameter power;    
 
        double lastPowerPro;    
        double lastError;       
        double rowPassDif;
        double output;              
        const double MAX_POWER;     
        const double INTERVAL_S;    
 
    public:
        SpeedPidController(double kPro, double kInt, double kDif,
                           unsigned long callInterval_us,
                           double maxPower = 10000) noexcept
            : constant{ kPro, kInt, kDif }
            , requestConstant{}
            , power{}
            , lastPowerPro(0.0)
            , lastError()
            , rowPassDif()
            , output()
            , MAX_POWER(maxPower)
            , 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 = (error - lastError) / INTERVAL_S * coefficient.p;
 
            // 積分量の計算
            power.i = error * coefficient.i;
 
            // 微分量の計算
            power.d = (power.p - lastPowerPro) / INTERVAL_S;
 
            // ローパスフィルタの計算
            rowPassDif += (power.d - rowPassDif) / 8 * coefficient.d;
 
            const double deltaPower = power.p + power.i + rowPassDif;
 
            output = Udon::Constrain(output + deltaPower, -MAX_POWER, MAX_POWER);
 
            // 偏差の保存
            lastPowerPro = power.p;
            lastError = error;
        }

        double getPower() const noexcept
        {
            return output;
        }

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

        double getPower(const Udon::Range<double>& range) const noexcept
        {
            return Udon::Constrain(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 = {};
            output = 0.0;
            lastPowerPro = 0.0;
            rowPassDif = 0.0;
            lastError = 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