Diff: main.cpp

Revision:

3:07fedd2e252c

Parent:

2:d7286c36595f

Child:

4:e7d50c6a7c53

--- a/main.cpp	Fri Oct 11 11:49:34 2019 +0000
+++ b/main.cpp	Fri Oct 11 12:02:22 2019 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
-//#include "HIDScope.h"
+#include "HIDScope.h"
 #include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
+#include "BiQuad.h"
 #include "FastPWM.h"
 
 // Button and potmeter control
@@ -33,12 +33,13 @@
 double potValue;
 double pi2= 6.283185;
 float e; //e = error
-float Kp=0.1;
-float Ki=0.1;
-float Kd=0.7;
+float Kp=17.5;
+float Ki=1;
+float Kd=21.5;
 float u_k;
 float u_i;
 float u_d;
+
 // PC connection
 MODSERIAL pc(USBTX, USBRX);
 
@@ -60,6 +61,8 @@
 
 float PID_controller(float e){
     static float error_integral=0;
+    static float e_prev=e;
+    static BiQuad LowPassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
     
     //Proportional part:
     u_k=Kp*e;
@@ -68,8 +71,14 @@
     error_integral=error_integral+e*Ts;
     u_i=Ki*error_integral;
     
+    //Derivative part
+    float error_derivative =(e-e_prev)/Ts;
+    float filtered_error_derivative = LowPassFilter.step(error_derivative);
+    u_d=Kd*filtered_error_derivative;
+    e_prev=e;
+    
     // Sum and return
-    return u_k+u_i;    
+    return u_k+u_i+u_d;    
 }
 
 void motorControl()