Diff: PID.cpp

Revision:

0:e3c87254ed02

diff -r 000000000000 -r e3c87254ed02 PID.cpp
--- /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