Diff: main.cpp

Revision:

1:3dfde431f833

Parent:

0:6fa73e77d49c

Child:

2:92aeffa6b16e

diff -r 6fa73e77d49c -r 3dfde431f833 main.cpp
--- a/main.cpp	Tue Oct 30 15:19:17 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:23:59 2018 +0000
@@ -23,11 +23,7 @@
 
 
 // Useful stuff
-Matrix H(3,3); // 2x2 matrix
-Matrix J2_2(3,1); //
 Ticker ForwardKinematics_ticker;
-float J2x_2;
-float J2y_2;
 
 void ForwardKinematics()
 {
@@ -55,52 +51,11 @@
     J2x_2 = L2*cos(q2);
     J2y_2 = L2*sin(q2);
 
-    //  Coordinate transformation for Joint 2
-    
+    // Calculation of Joint 2 expressed in frame 0
     float J1x_1 = L1*cos(motor_angle2);                              // Joint 1 expressed in frame 1
     float J1y_1 = L1*sin(motor_angle2); 
 
-    
-    H;
-    //float J2_1 = H*J2_2;                                  // Homogenous coordinates Joint 2 in frame 1
-    //float J2x_0 = J2_1(1) + L0 + L6;                              // x-coordinate Joint 2 in frame 0
-    //float J2y_0 = J2_1(2); 
-
-    // DEZE MATRIXMULTIPLICATIES MOETEN OOK IN EEN MATRIX FORMULE GEMAAKT WORDEN. MET STATIC VARIABLES KAN JE DAN NIEUWE MATRIX MAKEN
-    // DIE BESTAAT UIT DE COMPONENTEN VAN DE ANDERE MATRICES
-    
-    
-    
-}
-
-Matrix ComputeH(void)                                           // Making homogeneous matrix for frame 2 to 1 transformation
-{
-    double a = cos(motor_angle2);
-    double b = - sin(motor_angle2);
-    double c = L1*cos(motor_angle2);
-    double d = sin(motor_angle2);
-    double e = cos(motor_angle2);
-    double f = L1*sin(motor_angle2);
-    double g = 0;
-    double h = 0;
-    double i = 1;
-    
-    H << a << b << c
-        << d << e << f
-        << g << h << i;
-    return H;
-}
-
-Matrix ComputeJ2_2(void)                               // Homogenous coordinates Joint 2 in frame 2
-{
-    double a = J2x_2;
-    double b = J2y_2;
-    double c = 1;
-
-    J2_2 << a 
-        << b 
-        << c;
-    return J2_2;
+        
 }