Diff: main.cpp

Revision:

9:2944c792daa2

Parent:

8:01eb609fe0c1

Child:

10:c35d16ad1b6b

--- a/main.cpp	Thu Nov 01 10:29:23 2018 +0000
+++ b/main.cpp	Thu Nov 01 10:37:07 2018 +0000
@@ -45,6 +45,13 @@
     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 < 170 graden SAFETY LIMITATION
+    if (r > 2.96706)                                        // If delta is larger than 175 degrees than r is this
+    {
+        r = 0.2399;     // I CHANGED THIS
+        safetyLED = 0;
+    }
+
     // 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
@@ -83,14 +90,6 @@
         des_motor_angle2 = 1.48353;
         safetyLED = 0;
     }
-    
-    
-    // Delta < 170 graden
-    if (delta > 2.96706)                                        // If delta is larger than 180 degrees
-    {
-        r = 0.2399;     // I CHANGED THIS
-        safetyLED = 0;
-    }
 }