File content as of revision 0:5f9d8f2ef93e:

#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(){}