Diff: main.cpp

Revision:

4:89ebfa37663b

Parent:

2:0537a8007a39

Child:

5:7e59733b02b2

--- a/main.cpp	Thu Feb 08 22:55:15 2018 +0000
+++ b/main.cpp	Mon Mar 05 23:08:34 2018 +0000
@@ -1,4 +1,4 @@
-/*
+ /*
  * Code for the Module Microcontroller as part of the 
  * Autonomous Camera Kart project.
  * Controls the Gimbal motors, raise and lower mechanism,
@@ -19,15 +19,13 @@
 #include <Mx28.h>
 
 
-
-
 // Micro Module Declarations
-DigitalOut          LED(PA_5);
 PwmOut              liftSpeed(D10); // Normally D10
 DigitalOut          liftDirection(D11);
 DigitalIn           button(USER_BUTTON);
 Ticker              hallInt;
 Timer               filter_hall1;
+Timer               rosMode_Delay;
 DynamixelClass      gimbal(57600, D7, D8, D2);
 
 
@@ -40,25 +38,25 @@
 struct          fields rosInput;
 volatile int    currentPosition = 0;
 int             prevPosition = 0;
-bool            stall = false;
-
+int             stallcount = 0;
+bool            rosFlag = 1;
 
 // Main Program
 int main() {
     // Initializations
-    filter_hall1.start();                       //Start Filtering Timer 
+    filter_hall1.start();                       //Start Filtering Timer  
     setupGimbal();                              // Gimbal
     setupLift();                                // Lift
-    setupROSNode(nh, sub);                      // ROS Node Handle
-    
+    setupROSNode(nh, sub);                     // ROS Node Handle
+
     // Main Loop
     while (1) {     
-        // Lift Operation
+    // Lift Operation
         runLift(); 
-        //Run Gimbal 
+    //Run Gimbal 
         runGimbal();
-        //ROS Functions 
-        nh.spinOnce();  
-        rosCheck();      
-    }
+    //ROS Functions 
+        nh.spinOnce();
+        rosCheck();   
+    }    
 }