Diff: TestRKI.cpp

Revision:

1:11b728aa0077

Child:

2:8eca67634cba

--- /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;                      
+        
+    }
+}
+