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;  
        real_t Ki;  
        real_t Kd;  
    };
 
    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;
            real_t dt{ 1.0_re };
            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;
 
            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 ) noexcept;
 
            bool setAutoTuningControllerType( const pidType t ) noexcept;
 
            bool reset( void ) noexcept;
 
            inline pidGains getGains( void ) const noexcept
            {
                return { Kc, Ki, Kd };
            }
    };
 
}
 
#endif /*QLIBS_PID*/