Diff: main.cpp

Revision:

13:4f4cc1a5a79a

Parent:

12:146ba6f95fe0

--- a/main.cpp	Tue Oct 27 11:56:57 2015 +0000
+++ b/main.cpp	Wed Oct 28 14:29:18 2015 +0000
@@ -234,37 +234,57 @@
 double H2;
 double P1;
 double P2;
+// Safety stop. Motoren mogen niet verder dan 90 graden bewegen.
+volatile bool safety_stop;
 
 void move_motor1()
 {
-    if (final_filter1 > 0.04 || button_1 == pressed) {
+    if (safety_stop == true && (final_filter1 > 0.02 || button_1 == pressed)) {
         pc.printf("motor1 cw \n\r");
         motor1_direction = 0; //counterclockwise
-        motor1_speed = 0.2;
-    } else if (final_filter2 > 0.04 || button_2 == pressed) {
+        motor1_speed = 0.1;
+    } else if (safety_stop == true && (final_filter2 > 0.02 || button_2 == pressed)) {
         pc.printf("motor1 ccw \n\r");
         motor1_direction = 1; //clockwise
-        motor1_speed = 0.2;
+        motor1_speed = 0.1  ;
     } else {
-        pc.printf("Not moving \n\r");
+        pc.printf("Not moving1 \n\r");
         motor1_speed = 0;
     }
 }
 
 void move_motor2()
 {
-    if (final_filter3 > 0.05) {
+    if (safety_stop == true && (final_filter3 > 0.08 || button_1 == pressed)) {
         pc.printf("motor2 cw \n\r");
         motor2_direction = 1; //clockwise
         motor2_speed = 0.4;
-    } else if (final_filter4 > 0.05) {
+    } else if (safety_stop == true && (final_filter4 > 0.08 || button_2 == pressed)) {
         pc.printf("motor2 ccw \n\r");
         motor2_direction = 0; //counterclockwise
         motor2_speed = 0.4;
+        }
+         else if (P2 > 500) {
+        safety_stop = false;
+        pc.printf("Stop! /n/r");
+        motor2_direction = 1;
+        motor2_speed = 0.1;
+        wait(0.2);
+        safety_stop = true;
+        pc.printf("En door /n/r");
+    } else if (P2 < -500) {
+        safety_stop = false;
+        pc.printf("Stop! /n/r");
+        motor2_direction = 0;
+        motor2_speed = 0.1;
+        wait(0.2);
+        safety_stop = true;
+        pc.printf("En gaan /n/r");
     } else {
-        pc.printf("Not moving \n\r");
+        pc.printf("Not moving2 \n\r");
         motor2_speed = 0;
     }
+   
 }
 
 void movetohome()
@@ -302,6 +322,7 @@
 
 int main()
 {
+    safety_stop = true;
     while (true) {
         pc.baud(9600);                          //pc baud rate van de computer
         EMG_Filter.attach_us(Filters, 5e4);     //filters uitvoeren