Diff: ros_functions.cpp

Revision:

8:478f75c6109c

Parent:

7:950b3c3b5a2b

Child:

9:30901bec3a2d

--- a/ros_functions.cpp	Wed Mar 14 19:33:08 2018 +0000
+++ b/ros_functions.cpp	Thu Mar 15 19:20:23 2018 +0000
@@ -5,10 +5,9 @@
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32MultiArray.h>
 #include <prototypes.h>
-#include "std_msgs/Int8.h"
 
-int h = 0; //variable used to reset the currentPosition value when switching input direction
-int initialize = 0; //Variable used to allow the set up of each mode
+static int h = 0; //variable used to reset the currentPosition value when switching input direction
+static int initialize = 0; //Variable used to allow the set up of each mode
 int rosModeDelay = 1; //The amount of delay before switching modes 
 
 void module_commandCB(const std_msgs::Int32MultiArray& command_data){
@@ -23,11 +22,11 @@
 
 void rosCheck(){ 
         if(rosInput.mode == 0){ //Transport Mode
+            control.yaw = YAWZERO;
+            control.pitch = PITCHZERO;
+            control.roll = ROLLZERO;
+            control.height = LIFTHEIGHTMIN;
             if(initialize != rosInput.mode){
-                control.yaw = YAWZERO;
-                control.pitch = PITCHZERO;
-                control.roll = ROLLZERO;
-                control.height = LIFTHEIGHTMIN;
                 initializeMode();
             }
             else{
@@ -44,6 +43,7 @@
             }
             if(initialize != rosInput.mode){
                 rosFlag = 0;
+                control.height = currentPosition;
                 initialize = rosInput.mode;
                 rosMode_Delay.start(); 
             }