ResoLoopCutDistObsrv.hh

[詳解]

 
//
// Copyright (C) 2011-2022 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 RESOLOOPCUTDISTOBSRV
#define RESOLOOPCUTDISTOBSRV
 
#include <cassert>
#include "Matrix.hh"
#include "TransferFunction.hh"
#include "TwoInertiaParamDef.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 ResoLoopCutDistObsrv {
    public:
        ResoLoopCutDistObsrv(void)
            : Ds(), Ks(), Jm(), Dm(), Rg(), gd(), Ts(),
              Lnum(), Lden(), Rnum(), Rden(), Qnum(), Qden(), L(), R(), QLinv()
        {
            PassedLog();
        }
        
        ResoLoopCutDistObsrv(const struct TwoInertiaParams& Params, const double Bandwidth, const double SmplTime)
            : Ds(), Ks(), Jm(), Dm(), Rg(), gd(), Ts(),
              Lnum(), Lden(), Rnum(), Rden(), Qnum(), Qden(), L(), R(), QLinv()
        {
            SetParameters(Params, Bandwidth, SmplTime); // オブザーバのパラメータを設定
            PassedLog();
        }
        
        ResoLoopCutDistObsrv(ResoLoopCutDistObsrv&& r)
            : Ds(r.Ds), Ks(r.Ks), Jm(r.Jm), Dm(r.Dm), Rg(r.Rg), gd(r.gd), Ts(r.Ts),
              Lnum(r.Lnum), Lden(r.Lden), Rnum(r.Rnum), Rden(r.Rden), Qnum(r.Qnum), Qden(r.Qden), L(), R(), QLinv()
        {
            
        }
        
        ~ResoLoopCutDistObsrv(){
            PassedLog();
        }
        
        void SetParameters(const struct TwoInertiaParams& Params, const double Bandwidth, const double SmplTime){
            // パラメータの格納
            Ds = Params.Ds;
            Ks = Params.Ks;
            Jm = Params.Jm;
            Dm = Params.Dm;
            Rg = Params.Rg;
            gd = Bandwidth;
            Ts = SmplTime;
            
            // オブザーバ左側ループの伝達関数
            Lnum.Set( 1.0/Rg );
            Lden.Set( Jm, Dm );
            L.SetCoefficients(Lnum, Lden, Ts);
            
            // オブザーバ右側ループの伝達関数
            Rnum.Set( 1, 0 );
            Rden.Set( Ds, Ks );
            R.SetCoefficients(Rnum, Rden, Ts);
            
            // オブザーバフィルタと左側ループの逆系の乗算結果の伝達関数
            Qnum.Set( Rg*gd*Jm, Rg*gd*Dm );
            Qden.Set( 1, gd );
            QLinv.SetCoefficients(Qnum, Qden, Ts);
        }
        
        void SetBandwidth(const double Bandwidth){
            // パラメータの格納
            gd = Bandwidth;
            
            // オブザーバフィルタと左側ループの逆系の乗算結果の伝達関数
            Qnum.Set( Rg*gd*Jm, Rg*gd*Dm );
            Qden.Set( 1, gd );
            QLinv.SetCoefficients(Qnum, Qden, Ts);
        }
        
        double GetCompTorque(const double TorqueRef, const double TorsionTorque){
            return QLinv.GetResponse( L.GetResponse(TorqueRef) - R.GetResponse(TorsionTorque) );    // [Nm] モータ側トルク補償値の計算
        }
        
        void GetPDgainForRLCTTC(const double wt, const double zt, double& Kpt, double& Kdt){
            Kpt = -( (Jm*Ks - Dm*Ds)*Rg*wt*wt )/( 2.0*Ds*Ks*wt*zt - Ds*Ds*wt*wt - Ks*Ks );                      // [-] P-Dねじれトルク制御 比例ゲイン
            Kdt = -( 2.0*Jm*Ks*Rg*wt*zt - Ds*Jm*Rg*wt*wt - Dm*Ks*Rg )/( 2.0*Ds*Ks*wt*zt - Ds*Ds*wt*wt - Ks*Ks );// [-] P-Dねじれトルク制御 微分ゲイン
        }
        
    private:
        ResoLoopCutDistObsrv(const ResoLoopCutDistObsrv&) = delete;                 
        const ResoLoopCutDistObsrv& operator=(const ResoLoopCutDistObsrv&) = delete;
        double Ds;  
        double Ks;  
        double Jm;  
        double Dm;  
        double Rg;  
        double gd;  
        double Ts;  
        Matrix<1,1> Lnum;   
        Matrix<1,2> Lden;   
        Matrix<1,2> Rnum;   
        Matrix<1,2> Rden;   
        Matrix<1,2> Qnum;   
        Matrix<1,2> Qden;   
        TransferFunction<0,1> L;    
        TransferFunction<1,1> R;    
        TransferFunction<1,1> QLinv;
};
}
 
#endif