Diff: main.cpp

Revision:

7:f3c4ecbf864e

Parent:

6:273cea24fab3

Child:

8:01eb609fe0c1

--- a/main.cpp	Thu Nov 01 09:39:08 2018 +0000
+++ b/main.cpp	Thu Nov 01 10:01:25 2018 +0000
@@ -25,13 +25,15 @@
     float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5));      // Angle between L4 and L5 [rad]
     float q4 = PI - omega;                                                  // Angle of joint 3 between L3 and L4
     float theta = acos( -(pow(L5,2) - pow(n,2) - pow(L4,2))/(2*n*L4) );     // Angle between n and L4
-    float lambda = PI - atan(Pe_y/(L6-Pe_x));                               // Entire angle between L0 and n
+    float lambda_stuff = Pe_y/(L6-Pe_x);
+    float lambda = PI - atan(lambda_stuff);                               // Entire angle between L0 and n
     des_motor_angle1 = lambda - theta;
     float J3x_0 = L6 + L4*cos(des_motor_angle1);                            // x-coordinate of joint 3 in frame 0
     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 = J3y_0 - Pe_y;   // I CHANGED THIS!!!                                         // 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