TwoInertiaSimulators.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 TWOINERTIASIMULATORS
#define TWOINERTIASIMULATORS
 
#include <cassert>
#include <array>
#include "Matrix.hh"
#include "TwoInertiaParamDef.hh"
#include "TwoInertiaSimulator.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 <size_t N>
class TwoInertiaSimulators {
    public:
        TwoInertiaSimulators(const std::array<struct TwoInertiaParamDef, N>& Params, const double SmplTime)
            : PlantSyss()
        {
            PassedLog();
            // 各軸数分の2慣性系パラメータ設定
            for(size_t i = 0; i < N; ++i){
                PlantSyss.at(i).SetParameters(Params.at(i), SmplTime);
            }
        }
 
        TwoInertiaSimulators(TwoInertiaSimulators&& r)
            : PlantSyss(std::move(r.PlantSyss))
        {
            
        }
 
        ~TwoInertiaSimulators(){
            PassedLog();
        }
        
        void SetCurrentAndLoadTorque(const std::array<double, N>& Current, const std::array<double, N>& LoadTorque){
            // 各軸の電流と負荷トルクを設定
            for(size_t i = 0; i < N; ++i){
                PlantSyss.at(i).SetCurrentAndLoadTorque(Current.at(i), LoadTorque.at(i));
            }
        }
        
        void SetCurrentAndLoadTorque(const Matrix<1,N>& Current, const Matrix<1,N>& LoadTorque){
            std::array<double, N> iq, ts;
            Current.StoreArray(iq);
            LoadTorque.StoreArray(ts);
            SetCurrentAndLoadTorque(iq, ts);
        }
        
        void GetResponses(std::array<double, N>& LoadSpeed, std::array<double, N>& TorsionAngle, std::array<double, N>& MotorSpeed){
            // 各軸の応答を計算
            for(size_t i = 0; i < N; ++i){
                PlantSyss.at(i).GetResponses(LoadSpeed.at(i), TorsionAngle.at(i), MotorSpeed.at(i));
            }
        }
        
        void GetResponses(Matrix<1,N>& LoadSpeed, Matrix<1,N>& TorsionAngle, Matrix<1,N>& MotorSpeed){
            std::array<double, N> wl, ths, wm;
            GetResponses(wl, ths, wm);
            LoadSpeed.LoadArray(wl);
            TorsionAngle.LoadArray(ths);
            MotorSpeed.LoadArray(wm);
        }
        
        void GetMotorPosition(std::array<double, N>& MotorPosition){
            // 各軸の位置を取得
            for(size_t i = 0; i < N; ++i){
                MotorPosition.at(i) = PlantSyss.at(i).GetMotorPosition();
            }
        }
        
        void GetMotorPosition(Matrix<1,N>& MotorPosition){
            std::array<double, N> thm;
            GetMotorPosition(thm);
            MotorPosition.LoadArray(thm);
        }
        
        void GetLoadPosition(std::array<double, N>& LoadPosition){
            // 各軸の位置を取得
            for(size_t i = 0; i < N; ++i){
                LoadPosition.at(i) = PlantSyss.at(i).GetLoadPosition();
            }
        }
        
        void GetLoadPosition(Matrix<1,N>& LoadPosition){
            std::array<double, N> thl;
            GetLoadPosition(thl);
            LoadPosition.LoadArray(thl);
        }
        
    private:
        TwoInertiaSimulators(const TwoInertiaSimulators&) = delete;                 
        const TwoInertiaSimulators& operator=(const TwoInertiaSimulators&) = delete;
        
        std::array<TwoInertiaSimulator, N> PlantSyss;   
        
};
}
 
#endif