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 {
        friend class pidController;
        public:
            static const uint32_t UNDEFINED;
        protected:
            real_t p00{ 1.0_re };       /*covariance value*/
            real_t p01{ 0.0_re };       /*covariance value*/
            real_t p10{ 0.0_re };       /*covariance value*/
            real_t p11{ 1.0_re };       /*covariance value*/
            real_t b1{ 0.1_re };        /*estimation value*/
            real_t a1{ 0.9_re };        /*estimation value*/
            real_t uk{ 0.0_re };        /*process input*/
            real_t yk{ 0.0_re };        /*process output*/
            real_t l{ 0.9898_re };      /*memory factor [ 0.9 < l < 1 ]*/
            real_t k{ 1.0_re };         /*process static gain*/
            real_t tao{ 1.0_re };       /*process time constant*/
            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 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
            {
                l = 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;
    };

    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*/