PID.hpp

浏览该文件的文档.

#ifndef PID_HPP
#define PID_HPP
#include <type_traits>
#include <limits>
#include <cmath>
#include <algorithm>
 
#include "OneMotor/Util/Arithmetic.hpp"
#include "OneMotor/Util/DeltaT.hpp"
 
namespace OneMotor::Control
{
    template <Arithmetic ValueType_ = float>
    struct PID_Params
    {
        ValueType_ Kp{}; 
        ValueType_ Ki{}; 
        ValueType_ Kd{}; 
#ifdef ONE_MOTOR_LINUX
        ValueType_ MaxOutput{std::numeric_limits<ValueType_>::infinity()}; 
#else
        //TODO: 等Zephyr SDK更新看能不能修bug
        ValueType_ MaxOutput{};
#endif
 
        ValueType_ Deadband{}; 
#ifdef ONE_MOTOR_LINUX
        ValueType_ IntegralLimit{std::numeric_limits<ValueType_>::infinity()}; 
#else
        ValueType_ IntegralLimit{};
#endif
        ValueType_ DerivativeFilterRC{}; 
        ValueType_ OutputFilterRC{}; 
    };

    struct Positional
    {
    };

    struct Incremental
    {
    };
 
    // 特性标签
    struct WithDeadband // 死区
    {
    };

    struct WithIntegralLimit // 积分限幅
    {
    };

    struct WithDerivativeOnMeasurement // 微分先行
    {
    };

    struct WithDerivativeFilter // 微分项滤波
    {
    };

    struct WithOutputFilter // 输出滤波
    {
    };

    struct WithOutputLimit // 输出限幅
    {
    };

    template <typename... Fs>
    struct FeaturePack
    {
    };
 
    namespace Detail
    {
        template <typename Target, typename T>
        struct HasFeatureCheck : std::is_same<Target, T>
        {
        };
 
        // 特化：如果当前类型是 FeaturePack，则递归检查包内的所有类型
        template <typename Target, typename... Fs>
        struct HasFeatureCheck<Target, FeaturePack<Fs...>>
        {
            // 使用折叠表达式递归展开
            static constexpr bool value = (HasFeatureCheck<Target, Fs>::value || ...);
        };
    }

    template <typename Algorithm = Positional, Arithmetic ValueType = float, typename... Features>
    class PIDController
    {
        template <typename TargetFeature>
        static constexpr bool Check = (Detail::HasFeatureCheck<TargetFeature, Features>::value || ...);
 
        // 特性检查
        static constexpr bool PositionalPID = std::is_same_v<Algorithm, Positional>;
 
        static constexpr bool HasDeadband = Check<WithDeadband>;
        static constexpr bool HasIntegralLimit = Check<WithIntegralLimit>;
        static constexpr bool HasDerivativeOnMeasurement = Check<WithDerivativeOnMeasurement>;
        static constexpr bool HasDerivativeFilter = Check<WithDerivativeFilter>;
        static constexpr bool HasOutputFilter = Check<WithOutputFilter>;
        static constexpr bool HasOutputLimit = Check<WithOutputLimit>;
 
        // 状态变量
        ValueType ITerm{}; 
        ValueType prev_error{}; 
        ValueType prev_prev_error{}; 
        ValueType prev_ref{}; 
        ValueType prev_measure{}; 
        ValueType prev_derivative{}; 
        ValueType prev_output{}; 
        DeltaT<ValueType> deltaT{}; 
 
    public:
        using ParamsType = PID_Params<ValueType>;
        using ValType = ValueType;
        mutable ValueType MaxOutputVal; 
        mutable ValueType DeadbandVal; 
        mutable ValueType IntegralLimitVal; 
        mutable ValueType Kp; 
        mutable ValueType Ki; 
        mutable ValueType Kd; 
        mutable ValueType D_RC; 
        mutable ValueType O_RC; 

        explicit PIDController(const PID_Params<ValueType>& params) :
            MaxOutputVal(params.MaxOutput), DeadbandVal(params.Deadband), IntegralLimitVal(params.IntegralLimit),
            Kp(params.Kp), Ki(params.Ki), Kd(params.Kd),
            D_RC(params.DerivativeFilterRC), O_RC(params.OutputFilterRC)
        {
        }

        ValueType compute(ValueType ref, ValueType measure)
        {
            const ValueType error = ref - measure;
            if constexpr (HasDeadband)
            {
                if (std::abs(error) <= DeadbandVal)
                {
                    return prev_output;
                }
            }
            ValueType dt = deltaT.getDeltaMS();
            ValueType P, I{}, D, output;
            if constexpr (PositionalPID)
            {
                P = Kp * error;
                ITerm = Ki * error * dt;
            }
            else
            {
                P = Kp * (error - prev_error);
                ITerm = Ki * error * dt;
            }
 
            D = [&]
            {
                if constexpr (HasDerivativeOnMeasurement)
                {
                    ValueType meas_deriv = -(measure - prev_measure) / dt;
                    prev_measure = measure;
                    return meas_deriv;
                }
                else if constexpr (PositionalPID)
                {
                    return (error - prev_error) / dt;
                }
                else
                {
                    return (error - 2 * prev_error + prev_prev_error) / dt;
                }
            }();
            D *= Kd;
 
            // 应用微分滤波
            if constexpr (HasDerivativeFilter)
            {
                D = D * dt / (D_RC + dt) + prev_derivative * D_RC / (D_RC + dt);
            }
            // 应用积分限幅
            if constexpr (HasIntegralLimit)
            {
                ValueType temp_Iout = I + ITerm;
                if (const ValueType temp_Output = P + I + D; abs(temp_Output) > MaxOutputVal)
                {
                    if (error * I > 0)
                    {
                        ITerm = 0;
                    }
                }
                if (temp_Iout > IntegralLimitVal)
                {
                    ITerm = 0;
                    I = IntegralLimitVal;
                }
                if (temp_Iout < -IntegralLimitVal)
                {
                    ITerm = 0;
                    I = -IntegralLimitVal;
                }
            }
 
            I += ITerm;
            output = P + I + D;
 
            // 应用输出滤波
            if constexpr (HasOutputFilter)
            {
                output = output * dt / (O_RC + dt) + prev_output * O_RC / (O_RC + dt);
            }
 
            // 应用输出限幅
            if constexpr (HasOutputLimit)
            {
                output = std::clamp(output, -MaxOutputVal, MaxOutputVal);
            }
 
            prev_ref = ref;
            prev_derivative = D;
            prev_output = output;
            prev_error = error;
 
            return output;
        }

        void reset()
        {
            ITerm = ValueType{};
            prev_error = ValueType{};
            prev_prev_error = ValueType{};
            prev_ref = ValueType{};
            prev_measure = ValueType{};
            prev_derivative = ValueType{};
            prev_output = ValueType{};
 
            deltaT.reset();
        }
    };
}
 
#endif //PID_HPP