Diff: main.cpp

Revision:

34:bece2820b63b

Parent:

33:7b94346bcead

Child:

35:01aa6cb4a35f

diff -r 7b94346bcead -r bece2820b63b main.cpp
--- a/main.cpp	Thu Nov 01 16:09:00 2018 +0000
+++ b/main.cpp	Thu Nov 01 17:31:44 2018 +0000
@@ -68,23 +68,23 @@
 
 //Variables PID controller
 double PI = 3.14159;
-double Kp1 = 1.0;                                  //Motor 1           eerst 17.5 , nu 1
-double Ki1 = 1.0;
-double Kd1 = 23.0;
+double Kp1 = 20.0;                                  //Motor 1           eerst 17.5 , nu 1
+double Ki1 = 1.02;
+double Kd1 = 1.0;
 double encoder_radians1=0;
 double err_integral1 = 0;
 double err_prev1, err_prev2;
 double err1, err2;
 
-BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-//BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
-//BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
+//BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+//BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );  //sample frequency 500 Hz, cutoff 20Hz low pass
+BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
 
 
-double Kp2 = 1.0;                                  //Motor 2            eerst 17.5, nu 1
-double Ki2 = 1.0;
-double Kd2 = 23.0;
+double Kp2 = 20.0;                                  //Motor 2            eerst 17.5, nu 1
+double Ki2 = 1.02;
+double Kd2 = 1.0;
 double encoder_radians2=0;
 double err_integral2 = 0;
 double u1, u2;
@@ -100,7 +100,7 @@
 //------------------ Filter EMG + Calibration EMG --------------------------------//
 
 
-//---------PID controller motor 1 + motor control 1 & 2-----------------------------------------------------------//
+//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------//
 void PID_control1()
 {
     //pc.printf("ik doe het, PDI \n\r");