Diff: PID.cpp

Revision:

7:6823018ff785

Parent:

5:ceb7cbed26f3

diff -r 925e92d4d4e8 -r 6823018ff785 PID.cpp
--- a/PID.cpp	Sat Jul 20 13:26:09 2019 +0000
+++ b/PID.cpp	Sun Nov 10 01:41:03 2019 +0000
@@ -2,11 +2,12 @@
 #include "PID.h"
 
 /*p,i,d:ゲイン設定　t:制御ループ時間 max:ourputの最大値*/
-PID::PID(float p, float i, float d, float t, float max, Timer *T)
+PID::PID(float p, float i, float d, float t, float max, bool mode, Timer *T)
 {
     timer = T;
     kp = p; ki = i; kd = d; delta_t = t;
     abs_max_output = max;
+    pid_mode = mode;
     integral = 0;
     allowable_error = 0;
     
@@ -24,62 +25,58 @@
     pidTimer.attach(this, &PID::_compute, delta_t);
 }
 
-/*計算のための割り込み停止*/
-void PID::stop()
-{
-    reset();
-    pidTimer.detach();
-}
-
 /*現在保持している計算データをリセット*/
 void PID::reset()
 {
-    integral = 0;
-    error[0] = 0;
-    error[1] = 0;
+    output = integral = error[0] = error[1] = error[2] = 0;
 }
 
 /*計算する　タイマーで回される*/
 void PID::_compute()
-{    
+{
     float proportion, differential;
-    static float _output = 0;
-
-    error[0] = *target - *sensor;
+    float _output = 0;
+    
+    switch(pid_mode) {
+        case 0:   //位置型
+            error[1] = error[0];
+            error[0] = *target - *sensor;
+            proportion    = kp * error[0];
+            integral     += ki * (error[0] + error[1]) * delta_t / 2;
+            differential  = kd * (error[0] - error[1]) / delta_t;
+            _output = proportion + integral + differential;
+            break;
+        case 1:   //速度型
+            error[2] = error[1];
+            error[1] = error[0];
+            error[0] = *target - *sensor;
+            proportion    = kp * error[0] / delta_t;
+            integral      = ki * error[0];
+            differential  = kd * (error[0] - error[1] * 2 + error[2]) / delta_t;   //(前々-前) - (前-今) / t
+            _output += proportion + integral + differential;
+            break;
+        default:
+            break;
+    }
 
-    proportion    = kp * error[0];
-    integral     += ki * error[0] * delta_t;
-    differential  = kd * (error[0] - error[1]) / delta_t;
-
-    error[1] = error[0];
-
-    integral = _gurd(integral, abs_max_output);
-        
-    _output = proportion + integral + differential;
-    _output = _gurd(_output, abs_max_output);
-    output = _output;
+    integral = _gurd(integral);
+    output = _gurd(_output);
     
     last_target = *target;
     
-    if(_abs(error[0]) <= allowable_error)
+    if(abs(error[0]) <= allowable_error)
         timer->start();
     else start_time = timer->read();
 }
 
-float PID::_gurd(float val, float abs_max)
+float PID::_gurd(float val)
 {
-    if(val > abs_max)
-        return abs_max;
-    else if(val < -abs_max)
-        return -abs_max;
+    if(val > abs_max_output)
+        return abs_max_output;
+    else if(val < -abs_max_output)
+        return -abs_max_output;
     else return val;
 }
-float PID::_abs(float val)
-{
-    if(val >= 0)
-        return val;
-    else return -val;
-}
 
 bool PID::isConvergence(float time_range)
 {