Diff: PID.cpp

Revision:

0:da83c97448bf

Child:

1:4906167d1263

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Wed May 02 15:32:10 2018 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"
+#include "PID.h"
+
+/*p,i,d:ゲイン設定　t:制御ループ時間 max:ourputの最大値*/
+PID::PID(float p, float i, float d, float t, float max)
+{
+    kp = p; ki = i; kd = d; delta_t = t;
+    abs_max_output = max;
+    integral = 0;
+    pid_type = PID_LOCATE;
+}
+void PID::setParameter(float p, float i, float d, float t, float max)
+{
+    kp = p; ki = i; kd = d; delta_t = t;
+    abs_max_output = max;
+    integral = 0;
+}
+
+/*計算のための割り込み開始*/
+void PID::start()
+{
+    pidTimer.attach(this, &PID::_compute, delta_t);
+}
+
+/*計算のための割り込み停止*/
+void PID::stop()
+{
+    pidTimer.detach();
+}
+
+/*現在保持している計算データをリセット*/
+void PID::reset()
+{
+    integral = 0;
+    error[0] = 0;
+    error[1] = 0;
+}
+
+/*計算する　タイマーで回される*/
+void PID::_compute()
+{    
+    float proportion, differential;
+    static float _output = 0;
+
+    error[0] = *target - *sensor;
+
+    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);
+    
+    if(pid_type == PID_LOCATE)
+        _output = proportion + integral + differential;
+    else if(pid_type == PID_SPEED)
+        _output += proportion + integral + differential;
+    else _output = 0;
+    
+    _output = _gurd(_output, abs_max_output);
+    output = _output;
+}
+
+float PID::_gurd(float val, float abs_max)
+{
+    if(val > abs_max)
+        return abs_max;
+    else if(val < -abs_max)
+        return -abs_max;
+    else return val;
+}