Integrator.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 INTEGRATOR
#define INTEGRATOR
 
#include <cassert>
 
// 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名前空間
enum class IntegralType {
    FOWARD_EULER,   
    BACKWARD_EULER, 
    TRAPEZOIDAL     
};
 
template <IntegralType T = IntegralType::BACKWARD_EULER>
class Integrator {
    public:
        Integrator(void)
            : Ts(0),            // [s] 制御周期の格納
              uZ1(0), yZ1(0),   // 状態変数の初期化
              Ena(true)         // 積分器作動フラグ
        {
            PassedLog();
        }
        
        explicit Integrator(const double SmplTime)
            : Ts(SmplTime),     // [s] 制御周期の格納
              uZ1(0), yZ1(0),   // 状態変数の初期化
              Ena(true)         // 積分器作動フラグ
        {
            PassedLog();
        }
        
        Integrator(Integrator&& r)
            : Ts(r.Ts),                 // [s] 制御周期の格納
              uZ1(r.uZ1), yZ1(r.yZ1),   // 状態変数の初期化
              Ena(r.Ena)                // 積分器作動フラグ
        {
            
        }
        
        ~Integrator(){
            PassedLog();
        }
 
        double GetSignal(const double u){
            double y = 0;
            
            if(Ena == false)return yZ1; // 積分器停止指令の場合は積分動作を停止して積分値をそのまま出力
            
            // 積分方法によって計算を変える
            switch(T){
                case IntegralType::FOWARD_EULER:
                    y = Ts*uZ1 + yZ1;           // 前進オイラー(前進差分)
                    break;
                case IntegralType::BACKWARD_EULER:
                    y = Ts*u + yZ1;             // 後退オイラー(後退差分)
                    break;
                case IntegralType::TRAPEZOIDAL:
                    y = Ts/2.0*(u + uZ1) + yZ1; // 台形積分(双1次変換, Tustin変換)
                    break;
                default:
                    y = Ts*u + yZ1;             // デフォルトは後退オイラー
                    break;
            }
            
            // 状態変数の更新
            uZ1 = u;
            yZ1 = y;
            
            return y;
        }
 
        void SetSmplTime(const double SmplTime){
            Ts = SmplTime;  // [s] 制御周期の再設定
        }
 
        void SetInitial(const double InitValue){
            yZ1 = InitValue;
        }
        
        void SetIntegralValue(const double Value){
            yZ1 = Value;
        }
        
        void Enable(const bool Flag){
            Ena = Flag;
        }
 
        void ClearIntegralValue(void){
            uZ1 = 0;    // 状態変数1のゼロクリア
            yZ1 = 0;    // 状態変数2のゼロクリア
        }
        
    private:
        Integrator(const Integrator&) = delete;                 
        const Integrator& operator=(const Integrator&) = delete;
        double Ts;  
        double uZ1; 
        double yZ1; 
        bool Ena;   
    
};
}
 
#endif