Diff: inverse_kinematics.h

Revision:

27:b247e78a4380

Parent:

26:a8f4a117cc0d

Child:

50:5d2176b93a65

--- a/inverse_kinematics.h	Thu Nov 01 10:12:35 2018 +0000
+++ b/inverse_kinematics.h	Thu Nov 01 10:55:11 2018 +0000
@@ -31,6 +31,12 @@
     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
     
+    // Delta < 175 graden SAFETY LIMITATION
+    if (r > max_r_angle)                                        // If delta is larger than 175 degrees than r is this
+    {
+        r = max_r_angle;
+    }
+    
     // Calculation of motor_angle2
     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