Program Listing for File PID.cc

↰ Return to documentation for file (doxygen_docs/GeneralKModels/PID.cc)

#include"Karana/GeneralKModels/PID.h"
#include"Karana/KCore/Allocator.h"


namespaceKarana::Models{

namespacekd=Karana::Dynamics;

PID::PID(std::string_viewname,
constkc::ks_ptr<kd::StatePropagator>&sp,
constkc::ks_ptr<kd::PhysicalSubhinge>&sh,
std::function<km::Vec(constkm::Ktime&)>q_traj,
std::function<km::Vec(constkm::Ktime&)>u_traj)
:KModel<PID,PIDParams>(name,sp)
,_sh(sh)
,_q_traj(q_traj)
,_u_traj(u_traj){
params=std::allocate_shared<PIDParams>(kc::Allocator<PIDParams>{},
std::format("{}_params",name));

//Starttheinternalstateatzerosotheintegralgaincanbecalculatedappropriately
_e_prev.resize(sh->nQ());
_e_prev.setZero();

_integral_contrib.resize(sh->nQ());
_integral_contrib.setZero();

_prev_t=km::Ktime{0};
};

kc::ks_ptr<PID>PID::create(std::string_viewname,
constkc::ks_ptr<kd::StatePropagator>&sp,
constkc::ks_ptr<kd::PhysicalSubhinge>&sh,
std::function<km::Vec(constkm::Ktime&)>q_traj,
std::function<km::Vec(constkm::Ktime&)>u_traj){
kc::ks_ptr<PID>pid=
std::allocate_shared<PID>(kc::Allocator<PID>{},name,sp,sh,q_traj,u_traj);
sp->registerModel(pid);
returnpid;
}

voidPID::preDeriv(constkm::Ktime&t,constkm::Vec&){
km::VecT=params->kp*(_q_traj(t)-_sh->getQ())+
params->kd*(_u_traj(t)-_sh->getU())+_integral_contrib;
_sh->setT(T);

if(debug_model)[[unlikely]]{
stdDebugMsg(std::format("outputtorque{}",km::dumpString(T)));
}
}

voidPID::preModelStep(constkm::Ktime&t,constkm::Vec&){
//Calculatetheintegralcontributionforthisstep
_integral_contrib+=params->ki*(_e_prev*km::ktimeToSeconds(t-_prev_t));

//Calcualtenewerrorfornexttime
_e_prev=_q_traj(t)-_sh->getQ();

//Set_prev_ttotfornexttime
_prev_t=t;

if(debug_model)[[unlikely]]{
stdDebugMsg(std::format("preModelStepintegralerror{}",km::dumpString(_e_prev)));
}
};

PIDParams::PIDParams(std::string_viewname)
:KModelParams(name){
kp=km::uninitializedNaN;
kd=km::uninitializedNaN;
ki=km::uninitializedNaN;
}

boolPIDParams::isFinalized()const{
boolflag=true;
if(km::isUninitializedNaN(kp)){
kc::warn("Parameterkpisuninitialized.");
flag=false;
}
if(km::isUninitializedNaN(kd)){
kc::warn("Parameterkdisuninitialized.");
flag=false;
}
if(km::isUninitializedNaN(ki)){
kc::warn("Parameterkiisuninitialized.");
flag=false;
}
returnflag;
}

//DestructorincludedforMacOSbuilds.Musthaveakey-functionout-of-linetoavoiddulpicate
//symbols.
PID::~PID(){};
}//namespaceKarana::Models