Diff: TestRKI.cpp

Revision:

2:8eca67634cba

Parent:

1:11b728aa0077

--- a/TestRKI.cpp	Tue Oct 30 18:02:23 2018 +0000
+++ b/TestRKI.cpp	Tue Oct 30 18:21:31 2018 +0000
@@ -4,13 +4,14 @@
 // 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 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 
 const double T = 0.002f;                                  // Ticker value
-Ticker         ref_ticker;                          // Ticker aanmaken 
+Ticker ref_tick;                                          // Ticker aanmaken 
 
 
 // Variërende variabelen: 
@@ -29,22 +30,25 @@
 double q2_ii;                                       // Reference signal for motorangle q2 
 
 
-int main() {
-    while (true) {
-        
-        ticker.attach(ref_ticker);
-        
+void inverse_kinematics{
         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_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));     
+        q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));                                       
 
         q1_ii = q1 + q1_dot*T;                       
         q2_ii = q2 + q2_dot*T; 
         
         q1 = q1_ii;
-        q2 = q2_ii;                      
+        q2 = q2_ii; 
+        }
+
+
+int main() {
+    while (true) {
+        
+        ref_tick.attach(&inverse_kinematics,T);      
         
     }
 }