Diff: inverse_kinematics.h

Revision:

26:a8f4a117cc0d

Parent:

21:d541303a2ea6

Child:

27:b247e78a4380

--- a/inverse_kinematics.h	Thu Nov 01 09:10:21 2018 +0000
+++ b/inverse_kinematics.h	Thu Nov 01 10:12:35 2018 +0000
@@ -12,13 +12,14 @@
     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_pre_value = Pe_y/(L6-Pe_x);
+    float lambda = PI - atan(lambda_pre_value);                               // Entire angle between L0 and n
     dest_main_angle = lambda - theta;
     float J3x_0 = L6 + L4*cos(dest_main_angle);                            // x-coordinate of joint 3 in frame 0
     float J3y_0 = L4*sin(dest_main_angle);                                 // 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;                                           // 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
@@ -31,10 +32,9 @@
     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!
-    dest_sec_angle = gamma - beta;
+    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
+    dest_sec_angle = zeta - beta;
     
 
     // Determining angle delta for safety
@@ -44,7 +44,7 @@
     float m = sqrt(pow((J1x_0 - J3x_0),2) + pow((J3y_0 - J1y_0),2));        // Radius between Joint 1 and Joint 3
     float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3));     // Angle between L2 and L3
 
-
+    
     // Implementing stops for safety
     // 45 < Motor_angle1 < 70 graden
     if (dest_main_angle < main_arm_min_angle)                             // If motor_angle is smaller than 45 degrees
@@ -67,6 +67,7 @@
     }
     
     
+    
     // Delta < 170 graden
     if (delta > 2.96706)                                        // If delta is larger than 180 degrees
     {