Revision 3:6aa73c8a0489, committed 2019-10-18

Comitter:

s1961438

Date:

Fri Oct 18 12:37:38 2019 +0000

Parent:

2:8eca67634cba

Commit message:

Test 2 Inverse_Kinematics;

Changed in this revision

--- a/TestRKI.cpp	Tue Oct 30 18:21:31 2018 +0000
+++ b/TestRKI.cpp	Fri Oct 18 12:37:38 2019 +0000
@@ -1,14 +1,14 @@
 #include "mbed.h"
-#include <math.h> 
+#include <math.h>  //om dingen zoals sqrt te kunnen laten werken
 
 // Define variables 
 // Lengtes zijn in meters
 // Vaste variabelen:
-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 L1 = 0.05;                                   // Hoogte van base plaat tot joint 1 = afhankelijk van de patient
+const double L2 = 0.05;                                   // Hoogte van base plaat tot joint 2 = dus als hij in de startpositie staat is dit dezelfde hoogte als L2
+const double L3 = 0.30;                                   // Lengte van de arm 1
+//const double L4 = 0.089;                                // Afstand van achterkant base tot joint 1
+const double L5 = 0.30;                                   // Afstand van joint 1 naar joint 2 = arm 2
 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_tick;                                          // Ticker aanmaken 
@@ -21,7 +21,7 @@
 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 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 
@@ -30,7 +30,8 @@
 double q2_ii;                                       // Reference signal for motorangle q2 
 
 
-void inverse_kinematics{
+void inverse_kinematics
+{
         Lq1 = q1*r_trans;                            
         Cq2 = q2/5.0;                               
 
@@ -42,14 +43,14 @@
         
         q1 = q1_ii;
         q2 = q2_ii; 
-        }
+}
 
 
 int main() {
-    while (true) {
+    while (true)
+{
         
         ref_tick.attach(&inverse_kinematics,T);      
-        
-    }
+        }
 }