Diff: inverse_kinematics.h

Revision:

51:e0e4d7e3de93

Parent:

50:5d2176b93a65

--- a/inverse_kinematics.h	Mon Nov 05 10:14:31 2018 +0000
+++ b/inverse_kinematics.h	Mon Nov 05 16:03:39 2018 +0000
@@ -32,9 +32,9 @@
     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 then r is this
+    if (r > max_r_length)                                        // If delta is larger than 175 degrees then r is this
     {
-        r = max_r_angle;
+        r = max_r_length;
     }
     
     // Calculation of motor_angle2
@@ -43,7 +43,7 @@
     dest_sec_angle = zeta - beta;
     
 
-    // Determining angle delta for safety
+    // Determining angle delta for safety (not necessary for calculation, for debugging purposes)
     float J1x_0 = L0 + L6 + L1*cos(dest_sec_angle);                       // x-coordinate of joint 1 in frame 0
     float J1y_0 = L1*sin(dest_sec_angle);                                 // y-coordinate of joint 1 in frame 0   
     
@@ -52,7 +52,7 @@
 
     
     // Implementing stops for safety
-    // 45 < Motor_angle1 < 70 graden
+    // 45 < Motor_angle1 < 160 graden
     if (dest_main_angle < main_arm_min_angle)                             // If motor_angle is smaller than 45 degrees
     {
         dest_main_angle = main_arm_min_angle;