P-Dcontroller.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_DCONTROLLER
#define P_DCONTROLLER
 
#include <cassert>
#include "MovingDifferentiator.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 W = 4>
class P_Dcontroller {
    public:
        P_Dcontroller(void)
            : Diff(), Kp(0), Kd(0)
        {
            
        }
        
        P_Dcontroller(const double Pgain, const double Dgain)
            : Diff(), Kp(Pgain), Kd(Dgain)
        {
            EventLogVar(Kp);
            EventLogVar(Kd);
        }
 
        P_Dcontroller(P_Dcontroller&& r)
             : Diff(r.Diff), Kp(r.Kp), Kd(r.Kd)
        {
            
        }
 
        ~P_Dcontroller(){
            
        }
        
        double GetSignal(const double ref, const double res, const double time){
            return (ref - res)*Kp - Diff.GetSignal(res, time)*Kd;
        }
        
        void SetPgain(const double Pgain){
            Kp = Pgain;
        }
        
        void SetDgain(const double Dgain){
            Kd = Dgain;
        }
        
        void SetPDgain(const double Pgain, const double Dgain){
            SetPgain(Pgain);
            SetDgain(Dgain);
        }
        
    private:
        P_Dcontroller(const P_Dcontroller&) = delete;                   
        const P_Dcontroller& operator=(const P_Dcontroller&) = delete;  
        MovingDifferentiator<W> Diff;   
        double Kp;  
        double Kd;  
};
}
 
#endif