Revision 1:11b728aa0077, committed 2018-10-30

Comitter:

JorineOosting

Date:

Tue Oct 30 18:02:23 2018 +0000

Parent:

0:00721d3e1cf7

Commit message:

Project RKI test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TestRKI.cpp	Tue Oct 30 18:02:23 2018 +0000
@@ -0,0 +1,51 @@
+#include "mbed.h"
+#include <math.h> 
+
+// Define variables 
+// Lengtes zijn in meters
+// Vaste variabelen:
+const double L0 = 0.088;                                  // Afstand achterkant robot tot joint 2
+const double L1 = 0.288;                                  // Hoogte van tafel tot joint 2
+const double L2 = 0.208;                                  // Hoogte van tafel tot joint 1
+const double L3 = 0.212;                                  // Lengte van de arm
+const double r_trans = 0.035;                             // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 
+const double T = 0.002f;                                  // Ticker value
+Ticker         ref_ticker;                          // Ticker aanmaken 
+
+
+// Variërende variabelen: 
+double q1 = 0;                                      // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q2 = 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 effecftor 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;                                      // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
+double q2_dot;                                      // Benodigde hoeksnelheid van motor 2 om v_des te bereiken 
+
+double q1_ii;                                       // Reference signal for motorangle q1 
+double q2_ii;                                       // Reference signal for motorangle q2 
+
+
+int main() {
+    while (true) {
+        
+        ticker.attach(ref_ticker);
+        
+        Lq1 = q1*r_trans;                            
+        Cq2 = q2/5.0;                               
+
+        q1_dot = (v_x + (v_y*(L2 + L3*sin(q2/5.0)))/(L0 + q1*r_trans + L3*cos(q2/5.0)))/r_trans;     
+        q2_dot = (5.0*v_y)/(L0 + q1*r_trans + L3*cos(q2/5.0));                                       
+
+        q1_ii = q1 + q1_dot*T;                       
+        q2_ii = q2 + q2_dot*T; 
+        
+        q1 = q1_ii;
+        q2 = q2_ii;                      
+        
+    }
+}
+

--- a/main.cpp	Mon Oct 29 17:45:31 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,37 +0,0 @@
-#include "mbed.h"
-#include <math.h> 
-#include "Matrix.h"
-// Define variables 
-// Lengtes zijn in meters
-double L0 = 0.088;                                  // Afstand achterkant robot tot joint 2
-double L1 = 0.288;                                  // Hoogte van tafel tot joint 2
-double L3 = 0.212;                                  // Lengte van de arm
-// double r_trans;                                  // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 
-double q1;                                          // Translational joint, waarde uit encoder
-double q2;                                          // Rotational joint, waarde uit encoder --> q2 is 1/5 * waarde vd encoder (omrekeningsfactor)
-double v_x;                                         // Desired velocity end effector in x direction --> Determined by EMG signals
-double v_y;                                         // Desired velocity end effecftor in y direction --> Determined by EMG signals
-
-double J2_11 = 1.0;                                 // Berekeningen voor J'' komen uit MATLAB. [J2_11 J2_12; J2_21 J2_22] 
-double J2_12 = -L3*sin(q2);
-double J2_21 = 0.0; 
-double J2_22 = L3*cos(q2); 
-double T;                                           // Ticker value
-
-double q_dot_1 = v_x +(v_y*sin(q2))/cos(q2);        // Deze waarde gaat naar de motor
-double q_dot_2 = v_y/(L3*cos(q2)); 
-
-double q_t_1 = L0 + q1 + L3*cos(q2); 
-double q_t_2 = L1 + L3*sin(q2); 
-
-double q_tT_1 = L0 + q1 + T*(v_x + (v_y*sin(q2))/cos(q2)) + L3*cos(q2);
-double q_tT_2 = L1 + L3*sin(q2) + (T*v_y)/(L3*cos(q2));
-
-
-
-int main() {
-    while (true) {
-        
-    }
-}
-