Revision 7:6823018ff785, committed 2019-11-10

Comitter:

TanakaTarou

Date:

Sun Nov 10 01:41:03 2019 +0000

Parent:

6:925e92d4d4e8

Commit message:

Add Velocity

Changed in this revision

--- 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)
 {   

--- a/PID.h	Sat Jul 20 13:26:09 2019 +0000
+++ b/PID.h	Sun Nov 10 01:41:03 2019 +0000
@@ -3,22 +3,17 @@
 
 #include "mbed.h"
 
-//PID計算機
-/**/
-
 class PID
 {
 public:
-    PID(float p = 0, float i = 0, float d = 0, float t = 1, float max = 1.0, Timer *T = NULL);
+    PID(float p = 0, float i = 0, float d = 0, float t = 1, float max = 1.0, bool pid_mode = false, Timer *T = NULL);
     void setParameter(float p = 0, float i = 0, float d = 0, float t = 1, float max = 1.0);
     void start();
-    void stop();
     void reset();
     bool isConvergence(float timer_range);
     
     float output; //計算された出力値が入る　
 
-    float abs_max_output; //outputの100%を定義
     float *sensor, *target; //センサーと目標値を示す変数のポインタ 外側から引っ張ってくる
     float allowable_error;
     
@@ -26,12 +21,13 @@
     Timer *timer;
     Ticker pidTimer;
     float kp, ki, kd, delta_t;
-    float last_target;
+    float abs_max_output; //outputの100%を定義
+    bool pid_mode;   //0:位置型, 1:速度型
     float integral;
-    float error[2];
+    float error[3];   //現在の誤差, 前回の誤差, 前々回の誤差
+    float last_target;
     double start_time;
     void _compute();
-    float _gurd(float val, float abs_max);
-    float _abs(float val);
+    float _gurd(float val);
 };
 #endif

--- a/PID_VEL.cpp	Sat Jul 20 13:26:09 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,81 +0,0 @@
-#include "mbed.h"
-#include "PID_VEL.h"
-
-
-/*p,i,d:ゲイン設定　t:制御ループ時間  max:ourputの最大値*/
-PID_VEL::PID_VEL(float p, float i, float d, float t, float a_max, float max, Timer *T)
-{
-    kp = p; ki = i; kd = d; delta_t = t; acc_max_output = a_max; abs_max_output = max;
-    timer = T;
-    
-    integral = 0;
-    allowable_error = 0;
-}
-
-void PID_VEL::setParameter(float p, float i, float d, float t, float a_max, float max)
-{
-    kp = p; ki = i; kd = d; delta_t = t; acc_max_output = a_max; abs_max_output = max;
-    integral = 0;
-}
-
-/*計算のための割り込み開始*/
-void PID_VEL::start()
-{
-    pidTimer.attach(this, &PID_VEL::_compute, delta_t);
-}
-
-
-/*現在保持している計算データをリセット*/
-void PID_VEL::reset()
-{
-    integral = 0;
-    error = 0;
-    pre_error = 0;
-}
-
-/*一定周期で計算される*/
-void PID_VEL::_compute()
-{    
-    float proportion, differential;
-
-    error = *target - *sensor;
-    
-    proportion   = kp * error;
-    integral    += ki * error * delta_t;
-    differential = kd * (pre_error - error) / delta_t;
-    
-    _output += _gurd(proportion + integral + differential, acc_max_output);
-    if(_output > abs_max_output)
-        _output = _gurd(_output, abs_max_output);
-    else if(_output < 0)
-        _output = 0;
-    if(*sensor == 0 && *target == 0)
-        _output = integral = 0;
-    output = _output;
-    
-    if(abs(error) <= allowable_error)
-        timer->start();
-    else start_time = timer->read();
-    
-    last_target = *target;
-    pre_error = error;
-    pre_proportion = proportion;
-}
-
-float PID_VEL::_gurd(float val, float max_val)
-{
-    if(val > max_val)
-        return max_val;
-    else if(val < -max_val)
-        return -max_val;
-    else return val;
-}
-
-bool PID_VEL::isConvergence(double time_range)
-{
-    if(last_target != *target)
-        return 0; 
-    if(timer->read() - start_time > time_range)
-        return 1;
-    else return 0;
-}
\ No newline at end of file

--- a/PID_VEL.h	Sat Jul 20 13:26:09 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,39 +0,0 @@
-#ifndef PID_VEL_H
-#define PID_VEL_H
-
-#include "mbed.h"
-
-//PID計算機
-/**/
-
-class PID_VEL
-{
-public:
-    PID_VEL(float p, float i, float d, float t, float a_max, float max, Timer *T);
-    void setParameter(float p, float i, float d, float t, float a_max, float max);
-    void start();
-    void reset();
-    bool isConvergence(double timer_range);
-    
-    float output; //計算された出力値が入る　
-    float _output;
-    float error, pre_error;
-
-    float *sensor, *target; //センサーと目標値を示す変数のポインタ 外側から引っ張ってくる
-    float allowable_error;
-
-protected:
-    float delta_t, abs_max_output, acc_max_output;
-    void _compute();
-    float _gurd(float val, float max_val);    
-
-private:
-    Timer *timer;
-    Ticker pidTimer;
-    float kp, ki, kd;
-    float integral;
-    float last_target;
-    float pre_proportion;
-    double start_time;
-};
-#endif