Revision 0:e3c87254ed02, committed 2015-04-24

Comitter:

inst

Date:

Fri Apr 24 07:37:55 2015 +0000

Commit message:

?

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Fri Apr 24 07:37:55 2015 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "PID.h"
+
+const float PID::mPCoeff = 0.5f;
+const float PID::mICoeff = 0.1f;
+const float PID::mDCoeff = 10.0f;
+const float PID::mIRange = 0.2f;
+
+PID::PID(){
+    mI = 0.0f;
+    mOldDiff = 0.0f;
+}
+
+PID::~PID(){
+}
+
+void PID::updatePid( float diff ){
+    // P制御
+    float c = diff * mPCoeff;
+    
+    // I制御
+    if ( mOldDiff * diff < 0.0f ){
+        mI = 0;
+    }
+    if ( abs( diff ) < mIRange ){
+        c += mI * mICoeff;
+        mI += diff;
+    } else {
+        mI = 0;
+    }
+    
+    // D制御
+    c += ( diff - mOldDiff ) * mDCoeff;
+    
+    // 値の範囲を-1.0~1.0にする
+    if ( c > 1.0f ){
+        c = 1.0f;
+    } else if ( c < -1.0f ){
+        c = -1.0f;
+    }
+    
+    control( c );
+    
+    mOldDiff = diff;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Fri Apr 24 07:37:55 2015 +0000
@@ -0,0 +1,25 @@
+#ifndef INCLUDED_PID_H
+#define INCLUDED_PID_H
+
+class PID{
+public:
+    PID();
+    ~PID();
+    
+    void updatePid( float diff );
+    
+    float mOldDiff;
+
+private:
+    static const float mPCoeff;
+    static const float mICoeff;
+    static const float mDCoeff;
+    
+    static const float mIRange;
+    
+    virtual void control( float c ) = 0;
+    
+    float mI;
+};
+
+#endif
\ No newline at end of file