Diff: PID_controler.cpp

Revision:

2:f68fd7b1c655

Parent:

1:fafea1d00d0c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_controler.cpp	Thu Nov 01 11:13:32 2018 +0000
@@ -0,0 +1,30 @@
+#include "PID_controler.h"
+#include "mbed.h"
+#include "BiQuad.h"
+#include <math.h>
+#include <stdio.h>
+#include <iostream>
+#include <stdlib.h>
+#include <ctime>
+#include <QEI.h>
+
+double PID_controller(double error, double Kp, double Ki, double Kd, double Ts)
+{
+    static double error_integral = 0;
+    static double error_prev = error;
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+    //proportional part
+    double u_k = Kp * error;
+
+    //Integral part
+    error_integral = error_integral + error * Ts;
+    double u_i = Ki * error_integral;
+
+    // Derivative part
+    double error_derivative = (error - error_prev)/Ts;
+    double filtered_error_derivative = LowPassFilter.step(error_derivative);
+    double u_d = Kd * filtered_error_derivative;
+    error_prev = error;
+    return ((u_k + u_i + u_d)/5000);
+}
\ No newline at end of file