Diff: CalPID.cpp

Revision:

1:ce6f0674ab0d

Parent:

0:76fdba038ca7

Child:

3:634166991b10

diff -r 76fdba038ca7 -r ce6f0674ab0d CalPID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CalPID.cpp	Thu Mar 18 12:23:00 2021 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "CalPID.h"
+
+CalPID::CalPID(double kp_, double ki_, double kd_,double dt, double max)
+{
+    setParameter(kp_, ki_, kd_);
+    deviation_old = 0;
+    integral = 0;
+    delta_t=dt;
+    setMaxValue(max);
+};
+void CalPID::setParameter(double kp_, double ki_, double kd_)
+{
+    kp = kp_;
+    ki = ki_;
+    kd = kd_;
+}
+void CalPID::setMaxValue(double max)
+{
+    max_pid = max;
+}
+void CalPID::setDELTA_T(double delta_time)
+{
+    delta_t = delta_time;
+}
+double CalPID::calPID(double devia_present)
+{
+    double differential = (deviation_old - devia_present) / delta_t;
+    if(value_PID!=max_pid&&value_PID!=-max_pid) {//アンチワインドアップ（積分項固定式）
+        integral += (devia_present + deviation_old) * delta_t / 2;
+    }
+    value_PID = kp * devia_present + ki * integral - kd * differential;
+
+    if (value_PID > max_pid) {
+        value_PID = max_pid;
+    } else if (value_PID < -max_pid) {
+        value_PID = -max_pid;
+    }
+
+    deviation_old = devia_present;
+
+    return value_PID;
+}
+double CalPID::calPD(double devia_present)
+{
+    double differential = (deviation_old - devia_present) / delta_t;
+    double value_PD = kp * devia_present - kd * differential;
+
+    if (value_PD > max_pid) {
+        value_PD = max_pid;
+    } else if (value_PD < -max_pid) {
+        value_PD = -max_pid;
+    }
+
+    deviation_old = devia_present;
+
+    return value_PD;
+}
+double CalPID::calP_D(double devia_present,double diff_value)
+{
+    double value_P_D = kp * devia_present - kd * diff_value;
+
+    if (value_P_D > max_pid) {
+        value_P_D = max_pid;
+    } else if (value_P_D < -max_pid) {
+        value_P_D = -max_pid;
+    }
+
+    return value_P_D;
+}
+void CalPID::resetIntegral()
+{
+    integral = 0;
+}
\ No newline at end of file