P-Dcontrollers.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 P_DCONTROLLERS
#define P_DCONTROLLERS
 
#include <cassert>
#include <array>
#include "Matrix.hh"
#include "P-Dcontroller.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, size_t W = 4>
class P_Dcontrollers {
    public:
        P_Dcontrollers(void)
            : P_Dcon()
        {
            PassedLog();
        }
        
        P_Dcontrollers(const std::array<double, N>& Pgain, const std::array<double, N>& Dgain)
            : P_Dcon()
        {
            SetPgain(Pgain);
            SetDgain(Dgain);
            PassedLog();
        }
 
        P_Dcontrollers(const Matrix<1,N>& Pgain, const Matrix<1,N>& Dgain)
            : P_Dcon()
        {
            SetPgain(Pgain);
            SetDgain(Dgain);
            PassedLog();
        }
        
        P_Dcontrollers(P_Dcontrollers&& r)
             : P_Dcon(r.P_Dcon)
        {
            
        }
 
        ~P_Dcontrollers(){
            PassedLog();
        }
        
        void GetSignal(const Matrix<1,N>& ref, const Matrix<1,N>& res, const double time, Matrix<1,N>& out){
            for(size_t i = 1; i <= N; ++i){
                out[i] = P_Dcon.at(i-1).GetSignal(ref[i], res[i], time);
            }
        }
        
        Matrix<1,N> GetSignal(const Matrix<1,N>& ref, const Matrix<1,N>& res, const double time){
            Matrix<1,N> ret;
            GetSignal(ref, res, time, ret);
            return ret;
        }
        
        void SetPgain(const std::array<double, N>& Pgain){
            for(size_t i = 0; i < N; ++i) P_Dcon.at(i).SetPgain(Pgain.at(i));   // 各々のPゲインに設定
        }
        
        void SetDgain(const std::array<double, N>& Dgain){
            for(size_t i = 0; i < N; ++i) P_Dcon.at(i).SetDgain(Dgain.at(i));   // 各々のDゲインに設定
        }
        
        void SetPgain(const Matrix<1,N>& Pgain){
            for(size_t i = 1; i <= N; ++i) P_Dcon.at(i - 1).SetPgain(Pgain[i]); // 各々のPゲインに設定
        }
        
        void SetDgain(const Matrix<1,N>& Dgain){
            for(size_t i = 1; i <= N; ++i) P_Dcon.at(i - 1).SetDgain(Dgain[i]); // 各々のDゲインに設定
        }
        
        void SetPDgain(const std::array<double, N>& Pgain, const std::array<double, N>& Dgain){
            for(size_t i = 0; i < N; ++i) P_Dcon.at(i).SetPDgain(Pgain.at(i), Dgain.at(i)); // 各々のPDゲインに設定
        }
        
        void SetPDgain(const Matrix<1,N>& Pgain, const Matrix<1,N>& Dgain){
            for(size_t i = 1; i <= N; ++i) P_Dcon.at(i - 1).SetPDgain(Pgain[i], Dgain[i]);  // 各々のPDゲインに設定
        }
        
    private:
        P_Dcontrollers(const P_Dcontrollers&) = delete;                 
        const P_Dcontrollers& operator=(const P_Dcontrollers&) = delete;
        std::array<P_Dcontroller<W>, N> P_Dcon;                         
};
}
 
#endif