nakashima Kohei / NK_mbedsdffs

p

NK_PID.cpp@0:5f9d8f2ef93e, 2021-08-22 (annotated)

Committer:
hamohamo
Date:
Sun Aug 22 13:01:50 2021 +0000
Revision:
0:5f9d8f2ef93e
pp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hamohamo 0:5f9d8f2ef93e 1 #include "NK_PID.hpp"
hamohamo 0:5f9d8f2ef93e 2
hamohamo 0:5f9d8f2ef93e 3 Nk_pid::Nk_pid(double kp,double ki,double kd,double freq){
hamohamo 0:5f9d8f2ef93e 4 Kp=kp;
hamohamo 0:5f9d8f2ef93e 5 Ki=ki;
hamohamo 0:5f9d8f2ef93e 6 Kd=kd;
hamohamo 0:5f9d8f2ef93e 7 Freq=freq;
hamohamo 0:5f9d8f2ef93e 8 err[0]=0;
hamohamo 0:5f9d8f2ef93e 9 }
hamohamo 0:5f9d8f2ef93e 10 void Nk_pid::SetParam(double Value,double Target){
hamohamo 0:5f9d8f2ef93e 11
hamohamo 0:5f9d8f2ef93e 12 err[1]=(Target-Value)/(101.6/*タイヤの径*/*M_PI);
hamohamo 0:5f9d8f2ef93e 13 integral+=(err[1]+err[0])/2.0*Freq;
hamohamo 0:5f9d8f2ef93e 14 G_P=err[1];
hamohamo 0:5f9d8f2ef93e 15 G_I=integral;
hamohamo 0:5f9d8f2ef93e 16 G_D=(err[1]-err[0])/Freq;
hamohamo 0:5f9d8f2ef93e 17 err[0]=err[1];
hamohamo 0:5f9d8f2ef93e 18 res+=(Kp*G_P)+(Ki*G_I)+(Kd*G_D);
hamohamo 0:5f9d8f2ef93e 19 }
hamohamo 0:5f9d8f2ef93e 20 double Nk_pid::GetGain(double xvalue){
hamohamo 0:5f9d8f2ef93e 21 if(res<=0){
hamohamo 0:5f9d8f2ef93e 22 if(res<Max_M){
hamohamo 0:5f9d8f2ef93e 23 rtn=Max_M;
hamohamo 0:5f9d8f2ef93e 24 }
hamohamo 0:5f9d8f2ef93e 25 /*else if(res>=Min_M){
hamohamo 0:5f9d8f2ef93e 26 rtn=Min_M;
hamohamo 0:5f9d8f2ef93e 27 }*/
hamohamo 0:5f9d8f2ef93e 28 else{
hamohamo 0:5f9d8f2ef93e 29 rtn=res;
hamohamo 0:5f9d8f2ef93e 30 }
hamohamo 0:5f9d8f2ef93e 31 }
hamohamo 0:5f9d8f2ef93e 32 else {
hamohamo 0:5f9d8f2ef93e 33 if(res>Max_P){
hamohamo 0:5f9d8f2ef93e 34 rtn=Max_P;
hamohamo 0:5f9d8f2ef93e 35 }
hamohamo 0:5f9d8f2ef93e 36 /*else if(res<=Min_P){
hamohamo 0:5f9d8f2ef93e 37 rtn=Min_P;
hamohamo 0:5f9d8f2ef93e 38 }*/
hamohamo 0:5f9d8f2ef93e 39 else{
hamohamo 0:5f9d8f2ef93e 40 rtn=res;
hamohamo 0:5f9d8f2ef93e 41 }
hamohamo 0:5f9d8f2ef93e 42 }
hamohamo 0:5f9d8f2ef93e 43 return (rtn/xvalue);
hamohamo 0:5f9d8f2ef93e 44 }
hamohamo 0:5f9d8f2ef93e 45 void Nk_pid::SetLimit(double max_p,double min_p,double max_m,double min_m){
hamohamo 0:5f9d8f2ef93e 46 Max_P = max_p*1.0;
hamohamo 0:5f9d8f2ef93e 47 Min_P = min_p*1.0;
hamohamo 0:5f9d8f2ef93e 48 Max_M = max_m*1.0;
hamohamo 0:5f9d8f2ef93e 49 Min_M = min_m*1.0;
hamohamo 0:5f9d8f2ef93e 50 }
hamohamo 0:5f9d8f2ef93e 51 Nk_pid::~Nk_pid(){}