Revision 5:65a609067e14, committed 2018-10-31

Comitter:

MAHCSnijders

Date:

Wed Oct 31 14:11:08 2018 +0000

Parent:

4:6db7291caa6d

Child:

6:fe8712b56eb9

Commit message:

Safety checks fixed

Changed in this revision

--- a/main.cpp	Wed Oct 31 13:55:56 2018 +0000
+++ b/main.cpp	Wed Oct 31 14:11:08 2018 +0000
@@ -15,8 +15,8 @@
 const float L6 = 1.0;                       // Length beam between frame 0 and motor 1 [meter]
 volatile static float Pe_x_cur;             // Current x-coordinate of end-effector from frame 0 [meter]
 volatile static float Pe_y_cur;             // Current y-coordinate of end-effector from frame 0 [meter]
-volatile float motor_angle1;                // Current angle of motor 1 (left) based on kinematics [rad]
-volatile float motor_angle2;                // Current angle of motor 2 (right) based on kinematics [rad]
+volatile double motor_angle1;                // Current angle of motor 1 (left) based on kinematics [rad]
+volatile double motor_angle2;                // Current angle of motor 2 (right) based on kinematics [rad]
 
 DigitalOut safetyLED(LED_GREEN);             // Safety check LED
 
@@ -73,23 +73,34 @@
 
     // Implementing stops for safety
     // 45 < Motor_angle1 < 70 graden
-    if (0.785398 < motor_angle1 && motor_angle1 < 1.22173)
+    if (motor_angle1 < 0.785398)                                // If motor_angle is smaller than 45 degrees
     {
-        ForwardKinematics_ticker.detach();
+        motor_angle1 = 0.785398;
         safetyLED = 0;
-    }    
-     
-    // -42 < Motor_angle2 < 85 graden
-    if (-0.733038 < motor_angle2 && motor_angle2 < 1.48353)
+    }
+    else if (motor_angle1 > 1.22173)                            // If motor_angle is larger than 70 degrees
     {
-        ForwardKinematics_ticker.detach();
+        motor_angle1 = 1.22173;
         safetyLED = 0;
     }
-     
+    
+    // -42 < Motor_angle2 < 85 graden
+    if (motor_angle2 < -0.733038)                               // If motor_angle is smaller than -42 degrees
+    {
+        motor_angle2 = -0.733038;
+        safetyLED = 0;
+    }
+    else if (motor_angle2 > 1.48353)                            // If motor_angle is larger than 85 degrees
+    {
+        motor_angle2 = 1.48353;
+        safetyLED = 0;
+    }
+    
+    
     // Delta < 170 graden
-    if (delta < 2.96706)
+    if (delta > 2.96706)                                        // If delta is larger than 180 degrees
     {
-        ForwardKinematics_ticker.detach();
+        delta = 2.96706;
         safetyLED = 0;
     }