Diff: main.cpp

Revision:

6:273cea24fab3

Parent:

5:aaf68c7482bc

Child:

7:f3c4ecbf864e

--- a/main.cpp	Wed Oct 31 14:10:36 2018 +0000
+++ b/main.cpp	Thu Nov 01 09:39:08 2018 +0000
@@ -31,7 +31,7 @@
     float J3y_0 = L4*sin(des_motor_angle1);                                 // y-coordinate of joint 3 in frame 0
     
     // Calculation of the position of joint 2 in frame 0 
-    float S = abs(J3y_0 - Pe_y);                                            // Distance between height endeffector and joint 3
+    float S = J3y_0 - Pe_y;   // I CHANGED THIS!!!                                         // Distance between height endeffector and joint 3
     float kappa = asin(S/L5);                                               // Angle of L5  
     float J2x_0 = (L3+L5)*cos(kappa) + Pe_x;                                // x-coordinate of joint 2 in frame 0
     float J2y_0 = (L3+L5)*sin(kappa) + Pe_y;                                // y-coordinate of joint 2 in frame 0
@@ -43,11 +43,10 @@
     float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) );     // Angle opposite of radius r
     float q2 = PI - alfa;                                                   // Angle between L1 and L2
     
-    // Calculation of motor_angle2
-    float beta = atan(L2*sin(q2)/(L1+L2*cos(q2)));                          // Angle between r and L1
-    float gamma = PI - atan(abs(J2y_1/J2x_1));                              // Angle between r and x-axis
-    // check if gamma works!
-    des_motor_angle2 = gamma - beta;
+    // Calculation of motor_angle2     I CHANGED THIS!!!
+    float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r));       // Angle between r and L1
+    float zeta = acos(J2x_1/r);                                             // Angle between r and x-axis of frame 1
+    des_motor_angle2 = zeta - beta;
     
 
     // Determining angle delta for safety