Diff: main.cpp

Revision:

32:56a8bd82e971

Parent:

31:0418ce58af56

Child:

33:976be2825a23

diff -r 0418ce58af56 -r 56a8bd82e971 main.cpp
--- a/main.cpp	Thu Nov 01 18:04:50 2018 +0000
+++ b/main.cpp	Thu Nov 01 18:58:28 2018 +0000
@@ -82,28 +82,6 @@
 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
 
-// Inverse Kinematica variables
-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 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 
-
-// Variërende variabelen inverse kinematics: 
-double q1ref = 0.0;                                   // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
-double q2ref = 0.0;                                   // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
-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 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 
-
-double q1_ii=0.0;                                       // Reference signal for motorangle q1ref 
-double q2_ii=0.0;                                       // Reference signal for motorangle q2ref
 
 //Variables PID controller
 double PI = 3.14159;
@@ -124,6 +102,31 @@
 double u1, u2;
 BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
 
+// Inverse Kinematica variables
+//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 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 
+
+// Variërende variabelen inverse kinematics: 
+double q1ref = 0.0;                                   // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q2ref = 0.0;                                   // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
+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 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 
+
+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; 
 
 //--------------Functions----------------------------------------------------------------------------------------------------------------------------//
 
@@ -182,7 +185,7 @@
                 wait(0.001f);                           //Does there need to be a wait?
             }
             Mean0       = sum/sizeCal;                  //Calculate mean of the datapoints in the calibration set (2000 samples)
-            Threshold0  = Mean0/2;                      //Threshold calculation = 0.5*mean                                            
+            Threshold0  = Mean0*0.8;                    //Threshold calculation calve = 0.8*mean                                         
             break;                                      //Stop. Threshold is calculated, we will use this further in the code
         }
         case 1:                                         //If calibration state 1:
@@ -393,21 +396,23 @@
 //------------------ Inversed Kinematics --------------------------------//
 
 void inverse_kinematics()
-{
-    
+{    
     //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));                                          // Misschien gain toevoegen om te kijken of het dan werkt. Translatie gaat veel trager en moeizamer dan rotatie
 
-    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*5.0;
+    q2_motor = q2ref/r_trans;  
     
     
     //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);