Diff: actuators.cpp

Revision:

74:75be98779124

Parent:

69:37f75a7d36d8

Child:

76:0aa90e728e4a

--- a/actuators.cpp	Tue Oct 20 12:09:58 2015 +0200
+++ b/actuators.cpp	Tue Oct 20 12:14:18 2015 +0200
@@ -45,6 +45,9 @@
     bool pidOut = 0;
 
     // Create object instances
+    // Safety Pin
+    InterruptIn safetyInt(safetyPin);
+    
     // Initialze motors
     PwmOut motor1(motor1PWMPin);
     PwmOut motor2(motor2PWMPin);
@@ -66,6 +69,9 @@
     
 void motorInit(){
     
+    // connect safety to interrupt
+    safetyInt.rise(&safety);
+
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
 
@@ -149,7 +155,9 @@
 
 void servoControl(){
     // use potMeter Value to set servo angle
-    servo.write(servoPos);
+    if (motorsEnable){
+        servo.write(servoPos);
+    }
     // (optionaly calculate xy position to keep balloon in position)
         // calculate z position using angle
         // calculate x y translation of endpoint
@@ -181,4 +189,12 @@
     // // set the encoder values for angle.
     // encoder1.setValue(0);
     // encoder2.setValue(0);
+}
+
+
+safetyOn = false; // start with safety off for calibration
+void safety(){
+    if (safetyOn){
+        motorsEnable = false;
+    }
 }
\ No newline at end of file