Generator.hpp

Go to the documentation of this file.

#pragma once
#include <Eigen/Dense>
 
#include <controlpp/math.hpp>
 
namespace controlpp
{
    template<class T>
    class SineGenerator{
        private:
            T frequency_;
            T sample_time_;
            Eigen::Vector<T, 2> x_; 
            Eigen::Matrix<T, 2, 2> A_; 
 
        public:
 
            SineGenerator(const T& frequency, const T& sample_time)
                : frequency_(frequency)
                , sample_time_(sample_time)
                , x_(static_cast<T>(0), static_cast<T>(1))
            {
                this->set_frequency_and_sample_time(frequency, sample_time);
            }
 
            void set_frequency(const T& frequency){
                this->set_frequency_and_sample_time(frequency, this->sample_time_);
            }
 
            void set_sample_time(const T& sample_time){
                this->set_frequency_and_sample_time(this->frequency_, sample_time);
            }
 
            void set_frequency_and_sample_time(const T& frequency, const T& sample_time){
                // set members
                this->frequency_ = frequency;
                this->sample_time_ = sample_time;
 
                // continuous dynamic matrix
                Eigen::Matrix<T, 2, 2> A_continuous({
                    {0, frequency},
                    {-frequency, 0}
                });
 
                // discretised dynamic matrix
                Eigen::Matrix<T, 2, 2> ATs = A_continuous * sample_time;
                this->A_ = controlpp::expm(ATs);
            }
 
            T sin() const {return this->x_(0);}
 
            T cos() const {return this->x_(1);}
 
            Eigen::Vector<T, 2> next(){
                Eigen::Vector<T, 2> new_x = this->A_ * this->x_;
                this->x_ = new_x;
                return new_x;
            }
 
            void reset(){
                this->x_(0) = static_cast<T>(0);
                this->x_(1) = static_cast<T>(1);
            }
 
    };
} // namespace controlpp