Diff: main.cpp

Revision:

4:6db7291caa6d

Parent:

3:c006b3e9a41c

Child:

5:65a609067e14

--- a/main.cpp	Wed Oct 31 08:28:57 2018 +0000
+++ b/main.cpp	Wed Oct 31 13:55:56 2018 +0000
@@ -18,6 +18,8 @@
 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]
 
+DigitalOut safetyLED(LED_GREEN);             // Safety check LED
+
 
 // Useful stuff
 Ticker ForwardKinematics_ticker;
@@ -68,6 +70,29 @@
     float Pe_x = L6 - n*cos(rho);                                 // y-coordinate end-effector
     float Pe_y = n*sin(rho);                                      // x-coordinate end-effector
 
+
+    // Implementing stops for safety
+    // 45 < Motor_angle1 < 70 graden
+    if (0.785398 < motor_angle1 && motor_angle1 < 1.22173)
+    {
+        ForwardKinematics_ticker.detach();
+        safetyLED = 0;
+    }    
+     
+    // -42 < Motor_angle2 < 85 graden
+    if (-0.733038 < motor_angle2 && motor_angle2 < 1.48353)
+    {
+        ForwardKinematics_ticker.detach();
+        safetyLED = 0;
+    }
+     
+    // Delta < 170 graden
+    if (delta < 2.96706)
+    {
+        ForwardKinematics_ticker.detach();
+        safetyLED = 0;
+    }
+
     // NEEDS TO RETURN END-EFFECTOR COORDINATES
 //    return Pe_x;
 //    return Pe_y;    
@@ -76,7 +101,8 @@
 
 int main()
 {
+    safetyLED = 1;
     while (true) {
-    ForwardKinematics_ticker.attach(ForwardKinematics,2);
+    ForwardKinematics_ticker.attach(ForwardKinematics,0.5);
     }
 }
\ No newline at end of file