Revision 0:4ecdddd912e2, committed 2017-09-24

Comitter:

echo_piyo

Date:

Sun Sep 24 05:23:58 2017 +0000

Commit message:

????2??

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.cpp	Sun Sep 24 05:23:58 2017 +0000
@@ -0,0 +1,62 @@
+#include "pid.h"
+
+void PositionPid::setup(float Kp, float Ki, float Kd, float dt){
+    kp = Kp;
+    ki = Ki;
+    kd = Kd;
+    time = dt;
+    frequency = 1/dt;
+}
+
+void PositionPid::calculate(float target, float nowValue){
+    old = now;
+    now = nowValue - target;
+    p = now;
+    i = i + (now + old)*0.5*time;
+    d = (now - old) * frequency;
+    result = kp*p + ki*i + kd*d;
+    if (result > 1.0f) {
+        result = 1.0f;
+    } else if (result < -1.0f) {
+        result = -1.0f;
+    }
+}
+
+float PositionPid::duty(){
+    return result;
+}
+
+float PositionPid::duty_enableWidth(float duty_min, float duty_max){
+    if(now > duty_min && now < duty_max){   //nowは差
+        return 0.0;
+    }else{
+        return result;
+    }
+}
+
+void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){
+    kp = Kp;
+    ki = Ki;
+    kd = Kd;
+    time = dt;
+    frequency = 1/dt;
+}
+
+void SpeedPid::calculate(float target, float nowValue){
+    e2 = e1;
+    e1 = e;
+    e = nowValue - target;
+    p = e - e1;
+    i = e*time;
+    d = (e-2*e1+e2)*frequency;
+    result = result + (kp*p+ki*i+kd*d);
+    if (result > 1.0f) {
+        result = 1.0f;
+    } else if (result < -1.0f) {
+        result = -1.0f;
+    }
+}
+
+float SpeedPid::duty(){
+    return result;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.h	Sun Sep 24 05:23:58 2017 +0000
@@ -0,0 +1,62 @@
+/*********************************************
+PID library
+
+PositionPid : 位置型
+SpeedPid    : 速度型
+
+setup(float Kp, float Ki, float Kd, float dt)
+パラメータ、制御周期の設定
+Kp : Pゲイン
+Ki : Iゲイン
+Kd : Dゲイン
+dt : 制御周期[s]
+
+calculate(float target, float nowValue)
+PIDの計算をする
+target   : 目標値
+nowValue : 現在値
+
+duty()
+計算結果を-1~1で返す
+*********************************************/
+
+#ifndef MBED_PID_H
+#define MBED_PID_H
+
+#include "mbed.h"
+
+class PositionPid
+{
+public  :
+    void setup(float Kp, float Ki, float Kd, float dt);
+
+    void calculate(float target, float nowValue);
+
+    float duty();
+    
+    float duty_enableWidth(float duty_min, float duty_max);
+
+private :
+    float kp, ki, kd,
+          time, frequency,
+          old, now,
+          p, i, d, result;
+};
+
+class SpeedPid
+{
+public  :
+    void setup(float Kp, float Ki, float Kd, float dt);
+
+    void calculate(float target, float nowValue);
+
+    float duty();
+
+private :
+    float kp, ki, kd,
+          time, frequency,
+          e, e1, e2,
+          p, i, d, result;
+};
+
+#endif
\ No newline at end of file