p
Diff: NK_PID.cpp
- Revision:
- 0:5f9d8f2ef93e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/NK_PID.cpp Sun Aug 22 13:01:50 2021 +0000 @@ -0,0 +1,51 @@ +#include "NK_PID.hpp" + +Nk_pid::Nk_pid(double kp,double ki,double kd,double freq){ + Kp=kp; + Ki=ki; + Kd=kd; + Freq=freq; + err[0]=0; + } +void Nk_pid::SetParam(double Value,double Target){ + + err[1]=(Target-Value)/(101.6/*タイヤの径*/*M_PI); + integral+=(err[1]+err[0])/2.0*Freq; + G_P=err[1]; + G_I=integral; + G_D=(err[1]-err[0])/Freq; + err[0]=err[1]; + res+=(Kp*G_P)+(Ki*G_I)+(Kd*G_D); +} +double Nk_pid::GetGain(double xvalue){ + if(res<=0){ + if(res<Max_M){ + rtn=Max_M; + } + /*else if(res>=Min_M){ + rtn=Min_M; + }*/ + else{ + rtn=res; + } + } + else { + if(res>Max_P){ + rtn=Max_P; + } + /*else if(res<=Min_P){ + rtn=Min_P; + }*/ + else{ + rtn=res; + } + } + return (rtn/xvalue); +} +void Nk_pid::SetLimit(double max_p,double min_p,double max_m,double min_m){ + Max_P = max_p*1.0; + Min_P = min_p*1.0; + Max_M = max_m*1.0; + Min_M = min_m*1.0; +} +Nk_pid::~Nk_pid(){}