Diff: NK_PID.cpp

Revision:

0:5f9d8f2ef93e

diff -r 000000000000 -r 5f9d8f2ef93e NK_PID.cpp
--- /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(){}