Revision 0:5f9d8f2ef93e, committed 2021-08-22

Comitter:

hamohamo

Date:

Sun Aug 22 13:01:50 2021 +0000

Commit message:

pp

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NK_PID.hpp	Sun Aug 22 13:01:50 2021 +0000
@@ -0,0 +1,35 @@
+#ifndef NK_PID
+#define NK_PID
+
+#ifndef M_PI
+#define M_PI 3.14159265358979
+#endif
+
+class Nk_pid{
+public:
+  Nk_pid(double kp,double ki,double kd,double freq);
+  ~Nk_pid();
+  void SetParam(double Value,double Target);
+  void SetLimit(double max_p,double min_p,double max_m,double min_m);
+  double GetGain(double xvalue);
+  double res;
+  double value;
+  double target;
+  double err[2];
+private:
+  double Max_P;
+  double Min_P;
+  double Max_M;
+  double Min_M;
+  double Kp;
+  double Ki;
+  double Kd;
+  double Freq;
+  double G_P;
+  double G_I;
+  double G_D;
+  double integral;
+  double rtn;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NK_motor.cpp	Sun Aug 22 13:01:50 2021 +0000
@@ -0,0 +1,33 @@
+#include "NK_motor.hpp"
+
+Nk_motor::Nk_motor(PinName plus,PinName minus,int period){
+  Plus = new PwmOut(plus);
+  Minus = new PwmOut(minus);
+  Plus->period_us(period);
+  Minus->period_us(period);
+  /**Minus=0.0;
+  *Plus=0.0;*/
+}
+Nk_motor::~Nk_motor(){
+}
+void Nk_motor::SetLimit(double max,double min){
+  Max = max;
+  Min = min;
+}
+void Nk_motor::pwmout(double pwm){
+    if(pwm>=Max){
+        Pwm=Max;
+        }
+    else if(pwm<=Min){
+        Pwm=Min;
+        }
+    else{
+        Pwm=pwm;
+        }
+    if(Pwm<0.0){
+        *Minus=Pwm*-1;
+    }
+    else {
+        *Plus=Pwm;
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NK_motor.hpp	Sun Aug 22 13:01:50 2021 +0000
@@ -0,0 +1,20 @@
+#ifndef NK_motor
+#define NK_motor
+
+#include "mbed.h"
+
+class Nk_motor{
+public:
+  Nk_motor(PinName plus,PinName minus,int period);
+  ~Nk_motor();
+  void SetLimit(double max,double min);
+  void pwmout(double pwm);
+private:
+  double Max;
+  double Min;
+  PwmOut *Plus;
+  PwmOut *Minus;
+  double Pwm;
+};
+
+#endif