TwoInertiaStateFeedback.hh

[詳解]

 
//
// Copyright (C) 2011-2020 Yokokura, Yuki
// This program is free software;
// you can redistribute it and/or modify it under the terms of the FreeBSD License.
// For details, see the License.txt file.
 
#ifndef TWOINERTIASTATEFEEDBACK
#define TWOINERTIASTATEFEEDBACK
 
#include <cassert>
#include <complex>
#include "Matrix.hh"
#include "TwoInertiaParamDef.hh"
#include "TwoInertiaStateObsrv.hh"
 
// ARCS組込み用マクロ
#ifdef ARCS_IN
    // ARCSに組み込まれる場合
    #include "ARCSassert.hh"
    #include "ARCSeventlog.hh"
#else
    // ARCSに組み込まれない場合
    #define arcs_assert(a) (assert(a))
    #define PassedLog()
    #define EventLog(a)
    #define EventLogVar(a)
#endif
 
namespace ARCS {    // ARCS名前空間
//template <>
class TwoInertiaStateFeedback {
    public:
        TwoInertiaStateFeedback(
            const struct TwoInertiaParamDef& Params,
            const double Pole1, const double Pole2, const double Pole3,
            const double Bandwidth, const double SmplTime
        )
            : Kt(Params.Kt),
              Jm(Params.Jm),
              Dm(Params.Dm),
              Jl(Params.Jl),
              Dl(Params.Dl),
              Ks(Params.Ks),
              Rg(Params.Rg),
              gsob(Bandwidth),
              Ts(SmplTime),
              f(),
              SOB(Params, Bandwidth, SmplTime)
        {
            CalcStateFeedbackGain(Pole1, Pole2, Pole3);
            PassedLog();
        }
        
        TwoInertiaStateFeedback(
            const struct TwoInertiaParamDef& Params,
            const double Pole1, const std::complex<double>& Pole2, const std::complex<double>& Pole3,
            const double Bandwidth, const double SmplTime
        )
            : Kt(Params.Kt),
              Jm(Params.Jm),
              Dm(Params.Dm),
              Jl(Params.Jl),
              Dl(Params.Dl),
              Ks(Params.Ks),
              Rg(Params.Rg),
              gsob(Bandwidth),
              Ts(SmplTime),
              f(),
              SOB(Params, Bandwidth, SmplTime)
        {
            CalcStateFeedbackGain(Pole1, Pole2, Pole3);
            PassedLog();
        }
 
        TwoInertiaStateFeedback(TwoInertiaStateFeedback&& r)
            : Kt(r.Kt),
              Jm(r.Jm),
              Dm(r.Dm),
              Jl(r.Jl),
              Dl(r.Dl),
              Ks(r.Ks),
              Rg(r.Rg),
              gsob(r.gsob),
              Ts(r.Ts),
              f(r.f),
              SOB(std::move(r.SOB))
        {
            
        }
 
        ~TwoInertiaStateFeedback(){
            PassedLog();
        }
        
        double GetFeedbackCurrent(const double Current, const double Velocity){
            const Matrix<1,1> ireg = f*SOB.GetEstimatedVect(Current, Velocity);
            return ireg[1];
        }
        
    private:
        TwoInertiaStateFeedback(const TwoInertiaStateFeedback&) = delete;                   
        const TwoInertiaStateFeedback& operator=(const TwoInertiaStateFeedback&) = delete;  
        double Kt;  
        double Jm;  
        double Dm;  
        double Jl;  
        double Dl;  
        double Ks;  
        double Rg;  
        double gsob;
        double Ts;  
        Matrix<3,1> f;              
        TwoInertiaStateObsrv<> SOB; 
        
        void CalcStateFeedbackGain(double p1, double p2, double p3){
            const double fl = -(Jm*Rg*(Dl*Dl*Dl - 2.0*Dl*Jl*Ks + Dl*Dl*Jl*p1 + Dl*Dl*Jl*p2 + Dl*Dl*Jl*p3 - Jl*Jl*Ks*p1 - Jl*Jl*Ks*p2 - Jl*Jl*Ks*p3 + Dl*Jl*Jl*p1*p2 + Dl*Jl*Jl*p1*p3 + Dl*Jl*Jl*p2*p3 + Jl*Jl*Jl*p1*p2*p3))/(Jl*Jl*Ks*Kt);
            const double fs =  (Jm*Rg*(p1*p2 + p1*p3 + p2*p3 - Ks/Jl + Dl*Dl/(Jl*Jl) - Ks/(Jm*Rg*Rg) + (Dl*p1)/Jl + (Dl*p2)/Jl + (Dl*p3)/Jl))/Kt;
            const double fm = -(Dl*Jm + Dm*Jl + Jl*Jm*p1 + Jl*Jm*p2 + Jl*Jm*p3)/(Jl*Kt);
            f.Set(fl, fs, fm);
        }
        
        void CalcStateFeedbackGain(double p1, const std::complex<double>& p2, const std::complex<double>& p3){
            arcs_assert(p2 == conj(p3));    // 極がペアになってるかチェック
            const std::complex<double> fl = -(Jm*Rg*(Dl*Dl*Dl - 2.0*Dl*Jl*Ks + Dl*Dl*Jl*p1 + Dl*Dl*Jl*p2 + Dl*Dl*Jl*p3 - Jl*Jl*Ks*p1 - Jl*Jl*Ks*p2 - Jl*Jl*Ks*p3 + Dl*Jl*Jl*p1*p2 + Dl*Jl*Jl*p1*p3 + Dl*Jl*Jl*p2*p3 + Jl*Jl*Jl*p1*p2*p3))/(Jl*Jl*Ks*Kt);
            const std::complex<double> fs =  (Jm*Rg*(p1*p2 + p1*p3 + p2*p3 - Ks/Jl + Dl*Dl/(Jl*Jl) - Ks/(Jm*Rg*Rg) + (Dl*p1)/Jl + (Dl*p2)/Jl + (Dl*p3)/Jl))/Kt;
            const std::complex<double> fm = -(Dl*Jm + Dm*Jl + Jl*Jm*p1 + Jl*Jm*p2 + Jl*Jm*p3)/(Jl*Kt);
            f.Set(fl.real(), fs.real(), fm.real());
        }
        
};
}
 
#endif