Diff: main.cpp

Revision:

4:9f389b393af2

Parent:

3:f0208237b6f7

Child:

5:aaf68c7482bc

--- a/main.cpp	Tue Oct 30 19:51:44 2018 +0000
+++ b/main.cpp	Wed Oct 31 13:55:22 2018 +0000
@@ -14,6 +14,8 @@
 volatile static float des_motor_angle1;     // Desired angle of motor 1 (left) based on kinematics [rad]
 volatile static float des_motor_angle2;     // Desired angle of motor 2 (right) based on kinematics [rad]
 
+DigitalOut safetyLED(LED_BLUE);             // Safety check LED
+
 Ticker kinematics_ticker;                   // Ticker function for inverse kinematics
 
 void InverseKinematics()
@@ -54,10 +56,36 @@
     
     float m = sqrt(pow((J1x_0 - J3x_0),2) + pow((J3y_0 - J1y_0),2));        // Radius between Joint 1 and Joint 3
     float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3));     // Angle between L2 and L3
+
+
+    // Implementing stops for safety
+    // 45 < Motor_angle1 < 70 graden
+    if (0.785398 < des_motor_angle1 && des_motor_angle1 < 1.22173)
+    {
+        kinematics_ticker.detach();
+        safetyLED = 0;
+    }    
+     
+    // -42 < Motor_angle2 < 85 graden
+    if (-0.733038 < des_motor_angle2 && des_motor_angle2 < 1.48353)
+    {
+        kinematics_ticker.detach();
+        safetyLED = 0;
+    }
+     
+    // Delta < 170 graden
+    if (delta < 2.96706)
+    {
+        kinematics_ticker.detach();
+        safetyLED = 0;
+    }
 }    
 
 
 int main()
 {
+    safetyLED = 1;
+    while (true) {
     kinematics_ticker.attach(InverseKinematics,0.5);
+    }
 }
\ No newline at end of file