Diff: main.cpp

Revision:

10:836e701d00a6

Parent:

8:478f75c6109c

Child:

14:97804177806d

--- a/main.cpp	Mon Mar 26 21:32:30 2018 +0000
+++ b/main.cpp	Thu Apr 26 18:33:11 2018 +0000
@@ -25,6 +25,9 @@
 Timer               filter_hall1;
 Timer               rosMode_Delay;
 DynamixelClass      gimbal(57600, D7, D8, D2);
+InterruptIn         eStop(D5);
+DigitalOut          value0(D3); 
+DigitalOut          value1(D4);
 
 ros::NodeHandle     nh;
 ros::Subscriber<std_msgs::Int32MultiArray> sub("module_command", &module_commandCB);
@@ -32,32 +35,37 @@
 // Global Variable
 struct          fields control;
 struct          fields rosInput;
+float           liftDutyCycle = 0;
 volatile int    currentPosition = 0;
 int             prevPosition = 0;
 int             stallcount = 0;
 bool            rosFlag = 1;
+bool            eStopFlag = 0;
 
 // Main Program
 int main() {
     // Initializations
-    filter_hall1.start();                       //Start Filtering Timer  
+    value0.write(1);
+    value1.write(1);
+    filter_hall1.start();                       //Start Filtering Timer
+    eStop.fall(&eStopOn);
+    eStop.rise(&eStopOff);   
     setupGimbal();                              // Gimbal
     setupLift();                                // Lift
     setupROSNode(nh, sub);                     // ROS Node Handle
-    rosInput.yaw = 1;
-    rosInput.pitch = 1;
-    rosInput.roll = 1;
-    rosInput.height = 1;
     rosInput.mode = 0;
 
     // Main Loop
-    while (1) {     
+    while (1) { 
+    //LED Inputs
+        value0.write(0);
+        value1.write(0);
+    //ROS Functions 
+        nh.spinOnce();
+        rosCheck();    
     // Lift Operation
-        runLift(); 
+        runLift();
     //Run Gimbal 
         runGimbal();
-    //ROS Functions 
-        nh.spinOnce();
-        rosCheck();
-    }    
+    }     
 }