Revision 7:66e3f3fd08d8, committed 2016-08-07

Comitter:

kikoaac

Date:

Sun Aug 07 12:47:39 2016 +0000

Parent:

6:775c9421fe3b

Commit message:

PID

Changed in this revision

--- a/PID.cpp	Fri Oct 30 12:47:54 2015 +0000
+++ b/PID.cpp	Sun Aug 07 12:47:39 2016 +0000
@@ -53,10 +53,14 @@
     timer = p.timer;
     data = 0;
     bias=0;
+    
+    intF = false;
+    diffF = false;
+    propF = false;
     GAIN_P = p.GAIN_P;
     GAIN_I = p.GAIN_I;
     GAIN_D = p.GAIN_D;
-    setInterval(0.001);
+    setInterval(0.01);
     s_dErrIntg=0;
     dErr_prev=0;OutputLimits(1,0);
 }
@@ -68,7 +72,7 @@
     GAIN_P = tauKp;
     GAIN_I = tauKi;
     GAIN_D = tauKd;
-    setInterval(0.001);
+    setInterval(0.01);
     s_dErrIntg=0;
     dErr_prev=0;OutputLimits(1,0);
 }
@@ -96,7 +100,7 @@
     OutMax  = max;
     OutSpan = OutMax - OutMin;
 }
-void PID::setInterval(double inter)
+void PID::setInterval(float inter)
 {
     interval = inter;
     //start();
@@ -104,7 +108,7 @@
 void PID::start()
 {
     timer->start();
-    //T.attach(this,&PID::PIDctrl,interval);
+    T.attach(this,&PID::PIDctrl,interval);
     //printf("PID statr\n");
     //wait(0.1);
     //PIDctrl();
@@ -133,20 +137,24 @@
     dErr = dTarget - dPoint;
     float T=gettime();
     //printf("%f\t",T);
-    double dErrDiff = (dErr-data)/T;
+    double dErrDiff;
     // 誤差積分
-    if(data>OutMax)s_dErrIntg=OutMax;
-    else if(data<OutMin)s_dErrIntg=OutMin;
-    else s_dErrIntg += (dErr+dErr_prev )* T /2.0;
+    /*if(GAIN_I*s_dErrIntg>OutMax)s_dErrIntg=GAIN_I*OutMax;
+    else if(GAIN_I*s_dErrIntg<OutMin)s_dErrIntg=GAIN_I*OutMin;
+    else */
+    s_dErrIntg += (dErr+dErr_prev )* T /2.0;
     // 制御入力
-    dRet = bias+GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff;
+    dRet = bias + GAIN_P * (propF == true ? userProp : dErr) + 
+        GAIN_I * (intF == true ? userInt : s_dErrIntg) - 
+        GAIN_D * (diffF == true ? userDiff : dErrDiff);
     
     dErr_prev = dErr;
+    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());
+    //printf("PID %3.3f %3.3f %3.3f %3.3f %3.3f\r\n",data ,GAIN_P*dErr,GAIN_I*s_dErrIntg,GAIN_D*dErrDiff,T);
     
 }
 

--- a/PID.h	Fri Oct 30 12:47:54 2015 +0000
+++ b/PID.h	Sun Aug 07 12:47:39 2016 +0000
@@ -10,25 +10,34 @@
     PID& operator=(const PID &p) {
         return *this;
     }
+public:
+
     void PIDctrl();
-public:
     void InputLimits(float max,float min);
     void OutputLimits(float max,float min);
     PID(float tauKc, float tauKi, float tauKd ,Timer *T);
     void setbias(float bias_){bias=bias_;}
-    double s_dErrIntg ,dErr_prev;
+    float s_dErrIntg ,dErr_prev;
     void start();
     void pid_reset();
-    void setInterval(double inter);
+    void setInterval(float inter);
     //Getters.
     void stop();
-    double dTarget;
-    double dPoint;
+    float dTarget;
+    float dPoint;
     // PI制御ゲイン
-    double GAIN_P ;//1.5 // 比例ｹﾞｲﾝ
-    double GAIN_I ;//2.8 // 積分ｹﾞｲﾝ
-    double GAIN_D ;//0.2
-    double data;
+    float GAIN_P ;//1.5 // 比例ｹﾞｲﾝ
+    float GAIN_I ;//2.8 // 積分ｹﾞｲﾝ
+    float GAIN_D ;//0.2
+    float data;
+    double dErrDiff;
+    
+    float userInt;
+    float userDiff;
+    float userProp;
+    bool intF;
+    bool diffF;
+    bool propF;
 private:
     float bias;
     float OutMax;
@@ -38,13 +47,13 @@
     float OutSpan;
     float InSpan;
     Timer *timer;
+    float prev_time;
     float gettime() {
-        static float prev_time;
         float a = timer->read()-prev_time;
         prev_time=timer->read();
         return a;
     }
-    //Ticker T;
+    Ticker T;
     float interval;
 
 };