pid.hpp

 
#ifndef QLIBS_PID
#define QLIBS_PID
 
#include <include/qlibs_types.hpp>
#include <include/numa.hpp>

namespace qlibs {

    enum class pidMode {
        PID_AUTOMATIC,  
        PID_MANUAL,     
    };

    enum class pidType {
        PID_TYPE_P,     
        PID_TYPE_PI,    
        PID_TYPE_PD,    
        PID_TYPE_PID,   
    };

    enum class pidDirection {
        PID_FORWARD,    
        PID_BACKWARD,   
    };

    class pidController;

    struct pidGains {
        real_t Kc{ 0.0_re };  
        real_t Ki{ 0.0_re };  
        real_t Kd{ 0.0_re };  

        constexpr pidGains() = default;
        constexpr pidGains(real_t kc, real_t ki, real_t kd)
            : Kc(kc), Ki(ki), Kd(kd) {}
 
        constexpr pidGains operator+(const pidGains& other) const {
            return pidGains{
                this->Kc + other.Kc,
                this->Ki + other.Ki,
                this->Kd + other.Kd
            };
        }
    };

    constexpr pidGains operator"" _kc(long double v) {
        return {static_cast<real_t>(v), 0.0_re, 0.0_re};
    }
    constexpr pidGains operator"" _ki(long double v) {
        return {0.0_re, static_cast<real_t>(v), 0.0_re};
    }
    constexpr pidGains operator"" _kd(long double v) {
        return {0.0_re, 0.0_re, static_cast<real_t>(v)};
    }
 

    class pidAutoTuning {
        struct systemParams {
            real_t gain;
            real_t tau1;
            real_t tau2;
            bool stable;
        };
 
        struct estimationParams {
            real_t theta[4];    /*parameter estimations*/
            real_t P[4][4];     /*covariance matrix*/
            real_t lambda;      /*forgetting factor [ 0.9 < l < 1 ]*/
            real_t y1, y2;      /*past values of y(t)*/
            real_t u1, u2;      /*past values of u(t)*/
        };
 
        using EstimationStepFn = void(*)(estimationParams&, real_t);
        using ComputeParamsFn =  void(*)(systemParams&,const estimationParams&, const real_t);

        friend class pidController;
        public:
            static const uint32_t UNDEFINED;
        protected:
            EstimationStepFn estimationStep{ &pidAutoTuning::estimationStepN1 };
            ComputeParamsFn computeParameters{ &pidAutoTuning::computeParamsN1 };
            estimationParams estParams;
            systemParams sysParams;
 
            real_t mu{ 0.95_re };       /*variation attenuation*/
            real_t speed{ 0.25_re };    /*final controller speed*/
            uint32_t it{ UNDEFINED };   /*enable time*/
 
 
            static bool resetEstimation( estimationParams &p,
                                         const real_t r );
            static bool getTimeConstant( real_t& tau,
                                         const real_t abs_z,
                                         const real_t dt );
            static void computeParamsN1( systemParams& p,
                                         const estimationParams& e,
                                         const real_t dt );
            static void computeParamsN2( systemParams& p,
                                         const estimationParams& e,
                                         const real_t dt );
 
            static void estimationStepN1( estimationParams &p,
                                          const real_t y );
            static void estimationStepN2( estimationParams &p,
                                          const real_t y );
 
            static bool isValidValue( const real_t x ) noexcept;
            pidType type{ pidType::PID_TYPE_PI };
            void initialize( const pidGains current,
                             const real_t dt ) noexcept;
            inline void enable( const uint32_t tEnable ) noexcept
            {
                it = ( 0UL == tEnable ) ? pidAutoTuning::UNDEFINED : tEnable;
            }
            inline bool isComplete( void ) const noexcept
            {
                return ( ( 0UL == it ) && ( it != pidAutoTuning::UNDEFINED ) );
            }
            inline void setMemoryFactor( const real_t lambda ) noexcept
            {
                estParams.lambda = lambda;
            }
            inline void setMomentum( const real_t Mu ) noexcept
            {
                mu = Mu;
            }
            inline void setEstimatedControllerSpeed( const real_t alpha ) noexcept
            {
                speed = alpha;
            }
            inline void setEstimatedControllerType( const pidType t ) noexcept
            {
                type = t;
            }
            static inline bool isValidParam( const real_t p ) noexcept
            {
                return ( p > 0.0_re ) && ( p <= 1.0_re );
            }
            bool step( const real_t u,
                       const real_t y,
                       const real_t dt ) noexcept;
        public:
            pidAutoTuning() = default;
            pidGains getEstimates( void ) const noexcept;
            bool enableOrder2Estimates( const bool en ) noexcept;
    };

    class pidController : public pidGains, public nState, private nonCopyable {
        private:
            real_t b, c, sat_Min, sat_Max, epsilon, kw, kt, D, u1, beta, uSat, gainBlend;
            real_t dt{ 1.0_re };
            pidGains nextGains;
            real_t m, mInput;
            const real_t *yr{ nullptr };
            real_t alpha, gamma; /*MRAC additive controller parameters*/
            nState m_state; /*MRAC additive controller state*/
            nState b_state; /*Bumpless-transfer state*/
            pidAutoTuning *adapt{ nullptr };
            pidMode mode{ pidMode::PID_AUTOMATIC };
            pidDirection dir{ pidDirection::PID_FORWARD };
            bool isInitialized{ false };
            real_t error( real_t w, real_t y, real_t k = 1.0_re ) noexcept;
            static real_t saturate( real_t x,
                                    const real_t vMin,
                                    const real_t vMax ) noexcept;
            void adaptGains( const real_t u,
                             const real_t y ) noexcept;
        public:
            virtual ~pidController() {}
            pidController() = default;

            bool setup( const real_t kc,
                        const real_t ki,
                        const real_t kd,
                        const real_t dT ) noexcept;

            bool setup( const pidGains &g,
                        const real_t dT ) noexcept
            {
                return setup( g.Kc, g.Ki, g.Kd, dT );
            }

            bool setDirection( const pidDirection d ) noexcept;

            bool setParams( const real_t kc,
                            const real_t ti,
                            const real_t td ) noexcept;

            bool setGains( const real_t kc,
                           const real_t ki,
                           const real_t kd ) noexcept;

            bool setGains( const pidGains &g ) noexcept;

            bool setExtraGains( const real_t Kw,
                                const real_t Kt ) noexcept;

            bool setSaturation( const real_t Min,
                                const real_t Max ) noexcept;

            bool setSeries( void ) noexcept;

            bool setEpsilon( const real_t eps ) noexcept;

            bool setDerivativeFilter( const real_t Beta ) noexcept;

            bool setDerivativeFilterTimeConstant( const real_t Tf ) noexcept;

            bool setMode( const pidMode Mode ) noexcept;

            bool setReferenceWeighting( const real_t gb,
                                        const real_t gc ) noexcept;

            bool setManualInput( const real_t manualInput ) noexcept;

            bool setModelReferenceControl( const real_t &modelRef,
                                           const real_t Gamma = 0.5_re,
                                           const real_t Alpha = 0.01_re ) noexcept;

            bool removeModelReferenceControl( void ) noexcept;

            real_t control( const real_t w,
                            const real_t y ) noexcept;
 

            real_t operator()( const real_t w,
                               const real_t y )
            {
                return control( w, y );
            }

            bool bindAutoTuning( pidAutoTuning &at ) noexcept;

            inline bool unbindAutoTuning( void ) noexcept
            {
                const bool retValue = nullptr != adapt;
                adapt = nullptr;
                return retValue;
            }

            bool enableAutoTuning( const uint32_t tEnable ) noexcept;

            bool isAutoTuningComplete( void ) const noexcept;

            bool setAutoTuningParameters( const real_t Mu,
                                          const real_t Alpha,
                                          const real_t lambda,
                                          const real_t Tb = 3.0_re ) noexcept;

            bool setAutoTuningControllerType( const pidType t ) noexcept;

            bool reset( void ) noexcept;

            inline pidGains getGains( void ) const noexcept
            {
                return { Kc, Ki, Kd };
            }

            explicit operator bool() const noexcept {
                return isInitialized;
            }
    };

}
 
#endif /*QLIBS_PID*/