Diff: main.cpp

Revision:

16:1e91753f0a01

Parent:

15:1098bf926b5b

Child:

17:2b3fa9b1a05b

--- a/main.cpp	Thu May 02 09:19:54 2019 +0000
+++ b/main.cpp	Thu May 02 09:24:28 2019 +0000
@@ -1,7 +1,9 @@
 #include "mbed.h"
 #include "pin.h"
 #include "microinfinity.h"
-
+#include "pidcontroller.h"
+#include "debug.h"
+/*
 //#define DEBUG_ON
 
 #ifdef DEBUG_ON
@@ -9,7 +11,7 @@
 #else
 #define DEBUG(...)
 #endif
-
+*/
 #define Pi 3.14159265359 //円周率π
 
 enum WalkMode {
@@ -24,33 +26,6 @@
 };
 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
 
-class PIDcontroller //distanceをvalueに置き換えました
-{
-    float Kp_, Ki_, Kd_, tolerance_, time_delta_;
-    float pile_, value_old_, target_;
-
-public:
-    bool IsConvergence_;                                //収束したかどうか
-    PIDcontroller(float Kp, float Ki, float Kd);        //初期設定で係数を入力
-    void setCoefficients(float Kp, float Ki, float Kd)
-    {
-        Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
-    }; //係数を変更するときに使う
-    void setTimeDelta(float delta)
-    {
-        time_delta_ = delta;
-    };
-    void setTarget(float target);                       //目標位置の設定
-    void setTolerance(float tolerance)
-    {
-        tolerance_ = tolerance;
-    }; //許容誤差の設定
-    float calc(float nowVal);           //現在位置と目標を比較してPID補正
-    bool knowConvergence()
-    {
-        return IsConvergence_;
-    };             //収束したかどうかを外部に伝える
-};
 
 class Motor //PIDコントローラ、エンコーダを含むモータのクラス
 {
@@ -121,41 +96,6 @@
 
 
 
-PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
-{
-    Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
-    DEBUG("set Kp:%.3f  KI:%.3f  Kd:%.3f \n\r", Kp_, Ki_, Kd_);
-    IsConvergence_=true;
-}
-void PIDcontroller::setTarget(float target)
-{
-    if (IsConvergence_) { //収束時のみ変更可能
-        target_ = target;
-        DEBUG("set Target: %.3f\n\r", target_);
-        IsConvergence_ = false;
-    } else {
-        DEBUG("error: setTarget permission denied!\n");
-    }
-}
-float PIDcontroller::calc(float nowVal)
-{
-    float out = 0;
-    //PID計算ここで行う
-    float deviation = target_ - nowVal; //目標との差分
-    pile_ += deviation;                 //積分用に和を取る
-    out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
-    value_old_ = nowVal; //今のデータを保存
-    //
-    if (fabs(deviation) < tolerance_) { //収束した場合
-        DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
-        out = 0;
-        pile_ = 0;
-        value_old_ = 0;
-        IsConvergence_ = true;
-    }
-    return out;
-}
-
 Motor::Motor(PwmOut *forward, PwmOut *back)
 {
     pin_forward_ = forward;