Diff: PID.cpp

Revision:

0:4b3a2875ad1d

Child:

1:e2fb92398c1f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Wed Dec 30 16:08:25 2020 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "PID.h"
+
+PID::PID(float p, float i, float d) {
+    _p = p;
+    _i = i;
+    _d = d;
+
+    target_val = 0.0;
+    integral_val = 0.0;
+    last_time = 0; //us
+    last_val = 0.0;
+
+    timer.start();
+};
+
+float PID::get_input(float val) {
+    int now_time = timer.read_us();
+    float dt = now_time - last_time;
+    dt /= pow(10.0, 6.0);
+
+    val = val - target_val;
+    integral_val += (last_val * val) / 2 * dt;
+    float d_val = (val - last_val) / dt;
+
+    return _p * val+ _i * integral_val + _d * d_val;
+};
+
+void PID::set_target(float val) {
+    target_val = val;
+    reset_integral_val();
+};
+
+void PID::reset_integral_val() {
+    integral_val = 0.0;
+};
\ No newline at end of file