TimeVariantControl.hpp

Go to the documentation of this file.

#pragma once
#include <algorithm>
#include <limits>
 
#include <Eigen/Dense>
 
namespace controlpp
{
 
    namespace timevar{
 
        template<class T>
        struct ValueSampletimePair{
            T value;
            T sample_time;
        };
 
        template<class T>
        class PControl{
            public:
            using value_type = T;
            private:
 
            T kp_;
 
            public:
 
            constexpr PControl(const PControl&) = default;
 
            constexpr PControl(const T& kp) : kp_(kp){}
 
            constexpr void kp(const T& kp){this->kp_ = kp;}
 
            [[nodiscard]] constexpr const T& kp() const {return this->kp_;}
 
            constexpr T input(const T& u) {return u * this->kp_;}
 
            constexpr T input(const T& u, [[maybe_unused]]const T& Ts) {return this->input(u);}
 
            constexpr T operator() (const T& u) {return this->input(u);}
 
            constexpr T operator() (const T& u, [[maybe_unused]]const T& Ts) {return this->input(u);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, PControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset() {/*no operation*/}
        };
 
        
        template<class T>
        class IControl{
            public:
            using value_type = T;
            private:
            T ki_;
            T u_k1_ = static_cast<T>(0);
            T y_k1_ = static_cast<T>(0);
 
            public:
 
            constexpr IControl(const T& ki) : ki_(ki){}
 
            constexpr IControl(const IControl&) = default;
 
            constexpr void set_param(const T& ki) {return this->ki_ = ki;}
 
            constexpr void reset(){
                this->u_k1_ = static_cast<T>(0);
                this->y_k1_ = static_cast<T>(0);
            }
 
            constexpr T input(const T& u, const T& Ts){
                const T y = this->ki_ * Ts / 2 * (u + this->u_k1_) + this->y_k1_;
                this->y_k1_ = y;
                this->u_k1_ = u;
                return y;
            }
 
            constexpr T operator() (const T& u, const T& Ts){return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, IControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
        };
 
        template<class T>
        class IAntiWindup{
            public:
            using value_type = T;
            private:
            T ki_;
            T min_;
            T max_;
            T vn_;
            T vp_;
            
            T u_k1_ = static_cast<T>(0);
            T y_k1_ = static_cast<T>(0);
 
            public:
 
            constexpr IAntiWindup(const T& ki, const T& min, const T& max, const T& vn = std::numeric_limits<T>::lowest(), const T& vp = std::numeric_limits<T>::max()) 
                : ki_(ki)
                , min_(min)
                , max_(max)
                , vn_(vn)
                , vp_(vp)
                {}
 
            constexpr IAntiWindup(const IAntiWindup&) = default;
 
            constexpr void set_param(const T& ki) {return this->ki_ = ki;}
            
            constexpr void prate_limit(const T& pv) {return this->vp_ = pv;}
 
            [[nodiscard]] constexpr const T& prate_limit() const {return this->vp_;}
 
            constexpr void nrate_limit(const T& nv) {return this->vn_ = nv;}
 
            [[nodiscard]] constexpr const T& nrate_limit() const {return this->vn_;}
 
            constexpr void max_limit(const T& max) {return this->max_ = max;}
 
            [[nodiscard]] constexpr const T& max_limit() const {return this->max_;}
 
            constexpr void min_limit(const T& min) {return this->min_ = min;}
 
            [[nodiscard]] constexpr const T& min_limit() const {return this->min_;}
 
            constexpr void reset(){
                this->u_k1_ = static_cast<T>(0);
                this->y_k1_ = static_cast<T>(0);
            }
 
            constexpr T input(const T& u, const T& Ts){
                const T v = std::clamp(this->ki_ / 2 * (u + this->u_k1_), this->vn_, this->vp_); 
                const T dy = v * Ts;
                const T y = std::clamp(dy + this->y_k1_, this->min_, this->max_);
                
                this->y_k1_ = y;
                this->u_k1_ = u;
                return y;
            }
 
            constexpr T operator() (const T& u, const T& Ts){return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, IAntiWindup& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
        };
 
        template<class T>
        class DControl{
            private:
            T kd_;
            T omega_;
            T x_ = static_cast<T>(0);
 
            public:
 
            constexpr DControl(const T& kd, const T& omega)
                : kd_(kd)
                , omega_(omega){}
 
            constexpr DControl(const DControl&) = default;
 
            constexpr T input(const T& u, const T& Ts){
                const T V = 2 * kd_ * omega_;
                
                const T b0 = 1;
                const T b1 = -1;
 
                const T omega_Ts = omega_ * Ts;
                const T a0 = omega_Ts + 2;
                const T a1 = omega_Ts - 2;
 
                const T b0_ = b0 / a0;
                const T b1_ = b1 / a0;
 
                const T a1_ = a1 / a0;
 
                const T A = -a1_;
                const T C = b1_ - a1_ * b0_;
                const T D = b0_;
 
                const T new_x = A * x_ + u;
                const T y = C * x_ + D * u;
 
                this->x_ = new_x;
                return y * V;
            }
 
            constexpr T operator() (const T& u, const T& Ts) {return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, DControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                this->x_ = static_cast<T>(0);
            }
        };
 
        template<class T>
        class PT1Control{
            private:
            T _K = static_cast<T>(1);       
            T _omega = static_cast<T>(1);   
            T _x = static_cast<T>(0);       
        
            public:
            
            constexpr PT1Control(const T& K, const T& omega)
                : _K(K)
                , _omega(omega){}
 
            constexpr PT1Control(const PT1Control&) = default;
 
            constexpr void set_gain(const T& g){this->_K = g;}
            [[nodiscard]] constexpr const T& gain() const {return this->_K;}
 
            constexpr void set_omega(const T& w){this->_omega = w;}
            [[nodiscard]] constexpr const T& omega() const {return this->_omega;}
 
            constexpr T input(const T& u, const T& Ts){
                // helper values
                const T p = (Ts * this->_omega + 2);
                const T n = (Ts * this->_omega - 2);
 
                // parameters
                // a_0 = 1
                const T a = n / p;
                const T b = this->_K * Ts * this->_omega / p;
 
                // calculate new state and output
                const T new_x = (-a) * this->_x + u;
                const T new_y = b * ((1 - a) * this->_x + u);
 
                // update state and output
                this->_x = new_x;
                return new_y;
            }
 
            constexpr T operator() (const T& u, const T& Ts) {return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, PT1Control& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                this->_x = static_cast<T>(0);
            }
        };
 
        template<class T>
        class PT2Control{
            private:
            T K_; 
            T D_; 
            T omega_; 
            Eigen::Vector<T, 2> x_ = Eigen::Vector<T, 2>::Zero(); 
 
            public:
            PT2Control(const PT2Control&) = default;
 
            PT2Control(const T& K, const T& D, const T& omega)
                : K_(K)
                , D_(D)
                , omega_(omega){}
 
            constexpr T input(const T& u, const T& Ts){
                // calculate params of the tustin transformed transfer function
                const T b0 = static_cast<T>(1);
                const T b1 = static_cast<T>(2);
                const T b2 = static_cast<T>(1);
 
                const T omega_Ts_sqr = this->omega_ * this->omega_ * Ts * Ts;
                const T _4_DomegaTs = 4 * this->D_ * this->omega_ * Ts;
 
                const T V = this->K_ * omega_Ts_sqr;
                const T a0 = omega_Ts_sqr + _4_DomegaTs + 4;
                const T a1 = 2 * omega_Ts_sqr - 8;
                const T a2 = omega_Ts_sqr - _4_DomegaTs + 4;
 
                const T b0_ = b0 / a0;
                const T b1_ = b1 / a0;
                const T b2_ = b2 / a0;
 
                const T a1_ = a1 / a0;
                const T a2_ = a2 / a0;
 
                // turn it into state space form
                Eigen::Matrix<T, 2, 2> A({
                    {-a1_, -a2_},
                    {static_cast<T>(1), static_cast<T>(0)}
                });
 
                const Eigen::Matrix<T, 2, 1> B({
                    static_cast<T>(1),
                    static_cast<T>(0)
                });
 
                const Eigen::Matrix<T, 1, 2> C({
                    (b1_ - a1_ * b0_), (b2_ - a2_ * b0_)
                });
 
                const T D = b0_;
 
                // calculate the state space system response
                const Eigen::Vector<T, 2> new_x = A * this->x_ + B * u;
                const T y = C * this->x_ + D * u;
 
                // update states and output result
                this->x_ = new_x;
                return y * V;
            }
 
            constexpr T operator() (const T& u, const T& Ts) {return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, PT2Control& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                this->x_.setZero();
            }
        };
 
        template<class T>
        class PIControl{
            private:
            PControl<T> P_;
            IControl<T> I_;
 
            public:
 
            PIControl(const T& kp, const T& ki)
                : P_(kp)
                , I_(ki){}
 
            PIControl(const PIControl&) = default;
 
            constexpr T input(const T& u, const T& Ts){
                const T y = P_(u, Ts) + I_(u, Ts);
                return y;
            }
 
            constexpr T operator() (const T& u, const T& Ts) {return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, PIControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                this->P_.reset();
                this->I_.reset();
            }
        };
 
        template<class T>
        class PIAntiWindup{
            private:
            PControl<T> P_;
            IAntiWindup<T> I_;
 
            T y_k1_ = static_cast<T>(0);
 
            public:
 
            PIAntiWindup(const T& kp, const T& ki, const T& min, const T& max, const T& vn = std::numeric_limits<T>::lowest(), const T& vp = std::numeric_limits<T>::max())
                : P_(kp)
                , I_(ki, min, max, vn, vp)
                {}
 
            PIAntiWindup(const PIAntiWindup&) = default;
 
            constexpr void kp(const T& kp){this->P_.kp(kp);}
 
            [[nodiscard]] constexpr const T& kp() const {return this->P_.kp();}
 
            constexpr void ki(const T& ki){this->I_.ki(ki);}
 
            [[nodiscard]] constexpr const T& ki() const {return this->I_.ki_();}
 
            constexpr void prate_limit(const T& pv) {this->I_.prate_limit(pv);}
 
            [[nodiscard]] constexpr const T& prate_limit() const {return this->I_.prate_limit();}
 
            constexpr void nrate_limit(const T& nv) {this->I_.nrate_limit(nv);}
 
            [[nodiscard]] constexpr const T& nrate_limit() const {return this->I_.nrate_limit();}
 
            constexpr void max_limit(const T& max) {this->I_.max_limit(max);}
 
            [[nodiscard]] constexpr const T& max_limit() const {return this->I_.max_limit();}
 
            constexpr void min_limit(const T& min) {this->I_.min_limit(min);}
 
            [[nodiscard]] constexpr const T& min_limit() const {return this->I_.min_limit();}
 
            constexpr T input(const T& u, const T& Ts){
                // calculate new output
                T y_raw = I_.input(u, Ts) + P_.input(u);
                T rate_raw = (y_raw - y_k1_)/Ts;
                T rate = std::clamp(rate_raw, nrate_limit(), prate_limit());
                T y_rate_clamped = rate * Ts + y_k1_;
                T y = std::clamp(y_rate_clamped, min_limit(), max_limit());
 
                // update states
                this->y_k1_ = y;
 
                return y;
            }
 
            constexpr T operator()(const T& u, const T& Ts){return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, PIAntiWindup& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                this->I_.reset();
                this->y_k1_ = static_cast<T>(0);
            }
        };
 
 
        template<class T>
        class PT1IControl{
            private:
 
            PT1Control<T> PT1_;
            IControl<T> I_;
 
            public:
 
            constexpr PT1IControl(const PT1IControl&) = default;
 
            constexpr PT1IControl(const T& kp, const T& ki, const T& omega)
                : PT1_(kp, omega)
                , I_(ki)
                {}
 
            constexpr T input(const T& u, const T& Ts){
                // calculate filter parameters
                const T result = PT1_(u, Ts) + I_(u, Ts);
                return result;
            }
 
            constexpr T operator()(const T& u, const T& Ts){return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, PT1IControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                PT1_.reset();
                I_.reset();
            }
        };
 
        template<class T>
        class PIDControl{
            private:
            PControl<T> P_;
            IControl<T> I_;
            DControl<T> D_;
 
            public:
 
            PIDControl(const T& kp, const T& ki, const T& kd, const T& omega)
                : P_(kp)
                , I_(ki)
                , D_(kd, omega){}
 
            static PIDControl from_alpha_tune(const T& alpha, const T& omega_c, const T& mag_G){
                const T kp = 1 / (mag_G * alpha);
                const T ki = omega_c / (mag_G * alpha * alpha * alpha);
                const T kd = 1 / (omega_c * mag_G);
                const T omega = omega_c * alpha;
                PIDControl result(kp, ki, ki, omega);
                return result;
            }
 
            constexpr T input(const T& u, const T& Ts){
                const T result = P_(u, Ts) + I_(u, Ts) + D_(u, Ts);
                return result;
            }
 
            constexpr T operator() (const T& u, const T& Ts){return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, PIDControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                this->I_.reset();
                this->D_.reset();
            }
 
        };
 
        class PIDAntiWindup{
            //TODO
        };
 
        template<class T>
        class LeadLagControl{
            private:
            T omega1_; 
            T omega2_;
            T x_ = static_cast<T>(0);
 
            public:
 
            constexpr LeadLagControl(const T& omega1, const T& omega2)
                : omega1_(omega1)
                , omega2_(omega2){}
 
            constexpr T input(const T& u, const T& Ts){
                const T V = this->omega2_ / this->omega1_;
                
                const T b0 = Ts * this->omega1_ + static_cast<T>(2);
                const T b1 = Ts * this->omega1_ - static_cast<T>(2);
 
                const T a0 = Ts * this->omega2_ + static_cast<T>(2);
                const T a1 = Ts * this->omega2_ - static_cast<T>(2);
 
                const T b0_ = b0 / a0;
                const T b1_ = b1 / a0;
 
                const T a1_ = a1 / a0;
 
                const T A = -a1_;
                const T C = b1_ - a1_ * b0_;
                const T D = b0_;
 
                const T new_x = A * x_ + u;
                const T y = C * x_ + D * u;
 
                this->x_ = new_x;
                return y * V;
            }
 
            constexpr T operator() (const T& u, const T& Ts) {return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, LeadLagControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){this->x_ = static_cast<T>(0);}
        };
 
        template<class T>
        class NotchControl{
            private:
            T w_; 
            T g_min_; 
            T omega_; 
            Eigen::Vector<T, 2> x_ = Eigen::Vector<T, 2>::Zero();
 
            public:
 
            constexpr NotchControl(const T& w, const T& g_min, const T& omega)
                : w_(w)
                , g_min_(g_min)
                , omega_(omega){}
 
            constexpr T input(const T& u, const T& Ts){
                const T Ts_omega_sqr = Ts * Ts * this->omega_ * this->omega_;
                const T Ts_omega_sqr_plus_4 = Ts_omega_sqr + 4;
                const T _4_Ts_w_omega = 4 * Ts * this->w_ * this->omega_;
                const T _4_Ts_w_omega_g = _4_Ts_w_omega * this->g_min_;
 
                const T b0 = Ts_omega_sqr_plus_4 + _4_Ts_w_omega_g;
                const T b1 = 2 * (Ts_omega_sqr - 4);
                const T b2 = Ts_omega_sqr_plus_4 - _4_Ts_w_omega_g;
                
                const T a0 = Ts_omega_sqr_plus_4 + _4_Ts_w_omega;
                const T a1 = b1;
                const T a2 = Ts_omega_sqr_plus_4 - _4_Ts_w_omega;
 
                const T b0_ = b0 / a0;
                const T b1_ = b1 / a0;
                const T b2_ = b2 / a0;
 
                const T a1_ = a1 / a0;
                const T a2_ = a2 / a0;
 
                // turn it into state space form
                Eigen::Matrix<T, 2, 2> A({
                    {-a1_, -a2_},
                    {static_cast<T>(1), static_cast<T>(0)}
                });
 
                const Eigen::Matrix<T, 2, 1> B({
                    static_cast<T>(1),
                    static_cast<T>(0)
                });
 
                const Eigen::Matrix<T, 1, 2> C({
                    (b1_ - a1_ * b0_), (b2_ - a2_ * b0_)
                });
 
                const T D = b0_;
 
                // calculate the state space system response
                const Eigen::Vector<T, 2> new_x = A * this->x_ + B * u;
                const T y = C * this->x_ + D * u;
 
                // update states and output result
                this->x_ = new_x;
                return y;
            }
 
            constexpr T operator() (const T& u, const T& Ts) {return this->input(u, Ts);}
 
            friend constexpr ValueSampletimePair<T> operator>> (const ValueSampletimePair<T>& p, NotchControl& c){
                return ValueSampletimePair<T>{c.input(p.value, p.sample_time), p.sample_time};
            }
 
            constexpr void reset(){
                this->x_.setZero();
            }
        };
 
    } // vartime
} // namespace controlpp