Diff: lib/PID_Control.cpp

Revision:

0:e83b840a5f86

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/PID_Control.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,38 @@
+#include "PID_Control.h"
+
+PID_Control::PID_Control(double kp, double ki, double kd)
+{
+    set_PID(kp, ki, kd);
+    integral = 0.0;
+}
+
+double PID_Control::PID(double present, double target, double interval)
+{
+    diff[0] = diff[1];
+    diff[1] = target - present;
+    integral += ((diff[0] + diff[1]) / 2.0) * interval;
+    p = kp * diff[1];
+    i = ki * integral;
+    d = kd * (diff[1] - diff[0]);
+    control = p + i + d;
+    //printf("p[%f] i[%f] d[%f] ", p, i, d);
+    //printf("p:%.3f\t:%.3f\tc:%.3f\t:%.3f\r\n", present, target, control, interval);
+    //printf("t:%.3f\r\n", target);
+    return control;
+}
+
+void PID_Control::reset()
+{
+    diff[0] = 0;
+    diff[1] = 0.0;
+    integral = 0.0;
+    control = 0;
+}
+
+void PID_Control::set_PID(double kp, double ki, double kd)
+{
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+    reset();
+}