Diff: PID.cpp

Revision:

4:a3c85727f0f6

Parent:

3:34f4f22b18e7

diff -r 34f4f22b18e7 -r a3c85727f0f6 PID.cpp
--- a/PID.cpp	Tue Aug 18 04:45:10 2015 +0000
+++ b/PID.cpp	Mon Sep 14 07:43:57 2015 +0000
@@ -24,7 +24,7 @@
  * THE SOFTWARE.
  *
  * @section DESCRIPTION
- * 
+ *
  * A PID controller is a widely used feedback controller commonly found in
  * industry.
  *
@@ -48,16 +48,51 @@
  * Includes
  */
 #include "PID.h"
-
-PID::PID(float tauKp, float tauKi, float tauKd) 
+PID::PID(const PID& p)
 {
-
+    timer = p.timer;
+    data = 0;
+    GAIN_P = p.GAIN_P;
+    GAIN_I = p.GAIN_I;
+    GAIN_D = p.GAIN_D;
+    setInterval(0.001);
+    s_dErrIntg=0;
+    dErr_prev=0;OutputLimits(1,0);
+}
+PID::PID(float tauKp, float tauKi, float tauKd,Timer *T)
+{
+    timer=T;
     data = 0;
     GAIN_P = tauKp;
     GAIN_I = tauKi;
     GAIN_D = tauKd;
     setInterval(0.001);
-    s_dErrIntg=0; dErr_prev=0;
+    s_dErrIntg=0;
+    dErr_prev=0;OutputLimits(1,0);
+}
+
+void PID::InputLimits(float max,float min)
+{
+    //Make sure we haven't been given impossible values.
+    if (min >= max) {
+        return;
+    }
+
+
+    InMin  = min;
+    InMax  = max;
+    InSpan = InMax - InMin;
+}
+void PID::OutputLimits(float max,float min)
+{
+    if (min >= max) {
+        return;
+    }
+
+
+    OutMin  = min;
+    OutMax  = max;
+    OutSpan = OutMax - OutMin;
 }
 void PID::setInterval(double inter)
 {
@@ -66,44 +101,52 @@
 }
 void PID::Start()
 {
-    timer.start();
-    T.attach(this,&PID::PIDctrl,interval);
+    timer->start();
+    //T.attach(this,&PID::PIDctrl,interval);
     //printf("PID statr\n");
     //wait(0.1);
     //PIDctrl();
-    
+
 }
 void PID::stop()
 {
-    timer.stop();
-    T.detach();
+    timer->stop();
+    //T.detach();
 }
 
-void PID::Reset()
+void PID::pid_reset()
 {
-        dTarget=0;
-        dPoint=0;
-        // PI制御ゲイン
-        data=0;
-        s_dErrIntg=0 ,dErr_prev=0;
+    dTarget=0;
+    dPoint=0;
+    // PI制御ゲイン
+    data=0;
+    s_dErrIntg=0 ,dErr_prev=0;
 }
 void PID::PIDctrl()
 {
-    double dErr;
-    double dRet;
-    
+
     // 誤差
-    dErr = dTarget - dPoint;
+    T=gettime();
+    //printf("%f\t",T);
+    // 誤差積分
     
-    double dErrDiff = (dErr-dErr_prev)/timer.read();
-    //printf("PID %f,%f,%f,%f\n",dErr,s_dErrIntg,dErrDiff,timer.read());
-    // 誤差積分
-    s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;
+    dErr = dTarget - dPoint;
+    dErr_prev = dErr;
+    if(data>OutMax)s_dErrIntg=OutMax;
+    else if(data<OutMin)s_dErrIntg=OutMin;
+    else s_dErrIntg += (dErr+dErr_prev )* T /2.0;
     // 制御入力
-    dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff;
-    timer.reset();
-    dErr_prev = dErr;
-    data = dRet;
+    dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg - GAIN_D*dErrDiff;
+    dErrDiff = (dErr-data)/T;
+    
+    if(dRet>OutMax)data=OutMax;
+    else if(dRet<OutMin)data=OutMin;
+    else data = dRet;
+    
+    //printf("PID %f,%f,%f,%f,%f\r\n",data ,dErr,s_dErrIntg,dErrDiff,timer->read());
+    
 }
 
 
+
+