Diff: main.cpp

Revision:

36:bfdd1409855a

Parent:

35:01aa6cb4a35f

--- a/main.cpp	Fri Nov 02 07:59:57 2018 +0000
+++ b/main.cpp	Fri Nov 02 08:54:03 2018 +0000
@@ -44,10 +44,10 @@
 const float T2  = 0.002f;
 
 // Inverse Kinematica variables
-const double L1 = 0.208;                                  // Hoogte van tafel tot joint 1
+//const double L1 = 0.208;                                  // Hoogte van tafel tot joint 1
 //const double L2 = 0.288;                                  // Hoogte van tafel tot joint 2
 const double L3 = 0.212;                                  // Lengte van de arm
-const double L4 = 0.089;                                  // Afstand van achterkant base tot joint 1
+//const double L4 = 0.089;                                  // Afstand van achterkant base tot joint 1
 //const double L5 = 0.030;                                  // Afstand van joint 1 naar joint 2
 const double r_trans = 0.035;                             // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 
 
@@ -57,8 +57,8 @@
 double v_x;                                         // Desired velocity end effector in x direction --> Determined by EMG signals
 double v_y;                                         // Desired velocity end effector in y direction --> Determined by EMG signals
 
-double Lq1;                                         // Translatieafstand als gevolg van motor rotation joint 1
-double Cq2;                                         // Joint angle of the system (corrected for gear ratio 1:5)
+//double Lq1;                                         // Translatieafstand als gevolg van motor rotation joint 1
+//double Cq2;                                         // Joint angle of the system (corrected for gear ratio 1:5)
 
 double q1_dot=0.0;                                      // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
 double q2_dot=0.0;                                      // Benodigde hoeksnelheid van motor 2 om v_des te bereiken 
@@ -66,20 +66,23 @@
 double q1_ii=0.0;                                       // Reference signal for motorangle q1ref 
 double q2_ii=0.0;                                       // Reference signal for motorangle q2ref
 
+double q1_motor;
+double q2_motor; 
+
 //Variables PID controller
 double PI = 3.14159;
 double Kp1 = 20.0;                                  //Motor 1  
 double Ki1 = 1.02;
-double Kd1 = 1.0;
+double Kd1 = 5.0;
 double encoder_radians1=0;
 double err_integral1 = 0;
 double err_prev1, err_prev2;
 double err1, err2;
 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
 
-double Kp2 = 20.0;                                  //Motor 2
+double Kp2 = 1.0;                                  //Motor 2
 double Ki2 = 1.02;
-double Kd2 = 1.0;
+double Kd2 = 20.0;
 double encoder_radians2=0;
 double err_integral2 = 0;
 double u1, u2;
@@ -99,7 +102,6 @@
 //---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------//
 void PID_control1()
 {
-    //pc.printf("ik doe het, PDI \n\r");
 
     // Proportional part:
     double u_k1 = Kp1 * err1;
@@ -121,8 +123,7 @@
 
 void PID_control2()
 {
-    //pc.printf("ik doe het, PDI \n\r");
-
+    
     // Proportional part:
     double u_k2 = Kp2 * err2;
 
@@ -147,7 +148,7 @@
         encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
             //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
              //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
-        err1 = q1ref - encoder_radians1;
+        err1 = q1_motor - encoder_radians1;
             //pc.printf("err1 = %f\n\r",err1);
         PID_control1();                               //PID controller function call
         
@@ -169,7 +170,7 @@
         encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
             //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
             //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
-        err2 = q2ref - encoder_radians2;
+        err2 = q2_motor - encoder_radians2;
             //pc.printf("err2 = %f\n\r",err2);
         PID_control2();                            //PID controller function call
              //pc.printf("u2 = %f\n\r",u2);
@@ -194,18 +195,20 @@
     
     //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
     
-    Lq1 = q1ref*r_trans;                            
-    Cq2 = q2ref/5.0;                               
+    //Lq1 = q1ref*r_trans;                            
+    //Cq2 = q2ref/5.0;                               
 
-    q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));               //RKI systeem  
-    q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));                                          //
+    q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref);               //RKI systeem  
+    q2_dot = v_y/(L3*cos(q2ref));                                          //
 
-    q1_ii = q1ref + (q1_dot/r_trans)*T;                                             //Omgezet naar motorhoeken
-    q2_ii = q2ref + (q2_dot*5.0)*T; 
+    q1_ii = q1ref + q1_dot*T;                         //Omgezet naar motorhoeken
+    q2_ii = q2ref + q2_dot*T; 
         
     q1ref = q1_ii;
-    q2ref = q2_ii; 
+    q2ref = q2_ii;
     
+    q1_motor = q1ref/r_trans;
+    q2_motor = q2ref*5.0;      
     
     //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);