Diff: main.cpp

Revision:

33:7b94346bcead

Parent:

32:137d0f27e5a8

Child:

34:bece2820b63b

--- a/main.cpp	Thu Nov 01 14:40:34 2018 +0000
+++ b/main.cpp	Thu Nov 01 16:09:00 2018 +0000
@@ -40,8 +40,8 @@
                      
 
 //Global variables
-const float T   = 0.02f;                           //Ticker period             Deze wordt ook gebruikt in de PID, moet die niet anders???
-const float T2  = 0.02f;
+const float T   = 0.002f;                           //Ticker period             Deze wordt ook gebruikt in de PID, moet die niet anders???
+const float T2  = 0.002f;
 
 // Inverse Kinematica variables
 const double L1 = 0.208;                                  // Hoogte van tafel tot joint 1
@@ -68,9 +68,9 @@
 
 //Variables PID controller
 double PI = 3.14159;
-double Kp1 = 30.0;                                  //Motor 1           eerst 17.5 , nu 1
-double Ki1 = 1.3;
-double Kd1 = 2.0;
+double Kp1 = 1.0;                                  //Motor 1           eerst 17.5 , nu 1
+double Ki1 = 1.0;
+double Kd1 = 23.0;
 double encoder_radians1=0;
 double err_integral1 = 0;
 double err_prev1, err_prev2;
@@ -78,11 +78,13 @@
 
 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 );
 
 
-double Kp2 = 30.0;                                  //Motor 2            eerst 17.5, nu 1
-double Ki2 = 1.3;
-double Kd2 = 2.0;
+double Kp2 = 1.0;                                  //Motor 2            eerst 17.5, nu 1
+double Ki2 = 1.0;
+double Kd2 = 23.0;
 double encoder_radians2=0;
 double err_integral2 = 0;
 double u1, u2;