Diff: main.cpp

Revision:

5:aaf68c7482bc

Parent:

4:9f389b393af2

Child:

6:273cea24fab3

diff -r 9f389b393af2 -r aaf68c7482bc main.cpp
--- a/main.cpp	Wed Oct 31 13:55:22 2018 +0000
+++ b/main.cpp	Wed Oct 31 14:10:36 2018 +0000
@@ -60,23 +60,34 @@
 
     // Implementing stops for safety
     // 45 < Motor_angle1 < 70 graden
-    if (0.785398 < des_motor_angle1 && des_motor_angle1 < 1.22173)
+    if (des_motor_angle1 < 0.785398)                             // If motor_angle is smaller than 45 degrees
     {
-        kinematics_ticker.detach();
+        des_motor_angle1 = 0.785398;
         safetyLED = 0;
-    }    
-     
-    // -42 < Motor_angle2 < 85 graden
-    if (-0.733038 < des_motor_angle2 && des_motor_angle2 < 1.48353)
+    }
+    else if (des_motor_angle1 > 1.22173)                         // If motor_angle is larger than 70 degrees
     {
-        kinematics_ticker.detach();
+        des_motor_angle1 = 1.22173;
         safetyLED = 0;
     }
-     
+    
+    // -42 < Motor_angle2 < 85 graden
+    if (des_motor_angle2 < -0.733038)                            // If motor_angle is smaller than -42 degrees
+    {
+        des_motor_angle2 = -0.733038;
+        safetyLED = 0;
+    }
+    else if (des_motor_angle2 > 1.48353)                         // If motor_angle is larger than 85 degrees
+    {
+        des_motor_angle2 = 1.48353;
+        safetyLED = 0;
+    }
+    
+    
     // Delta < 170 graden
-    if (delta < 2.96706)
+    if (delta > 2.96706)                                        // If delta is larger than 180 degrees
     {
-        kinematics_ticker.detach();
+        delta = 2.96706;
         safetyLED = 0;
     }
 }