Diff: ros_functions.cpp

Revision:

6:2ffa254e8f6e

Parent:

4:89ebfa37663b

Child:

7:950b3c3b5a2b

--- a/ros_functions.cpp	Mon Mar 05 23:45:59 2018 +0000
+++ b/ros_functions.cpp	Tue Mar 06 21:44:07 2018 +0000
@@ -107,14 +107,15 @@
         }  
 }
 
+// Converting the ros inputs for the lift and gimbal and determining action
 void rosTranslator(int height, int yaw, int pitch, int roll){
     switch(height){
         case(0):
-        control.height = currentPosition;
+        control.height--;   
+        if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
         break;
         case(1):
-        control.height--;   
-        if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
+        control.height = currentPosition;
         break;
         case(2):
         control.height++;  
@@ -123,10 +124,10 @@
     } 
     switch(yaw){
         case(0):
+        control.yaw++;
+        if(control.yaw > YAWMAX){control.yaw = YAWMAX;}
         break;
         case(1):
-        control.yaw++;
-        if(control.yaw > YAWMAX){control.yaw = YAWMAX;}
         break;
         case(2):
         control.yaw--;
@@ -135,10 +136,10 @@
     }
         switch(pitch){
         case(0):
+        control.pitch--;
+        if(control.pitch < PITCHMIN){control.pitch = PITCHMIN;}
         break; 
         case(1):
-        control.pitch--;
-        if(control.pitch < PITCHMIN){control.pitch = PITCHMIN;}
         break;
         case(2):
         control.pitch++;
@@ -148,10 +149,10 @@
     }
     switch(rosInput.roll){
         case(0):
+        control.roll--;
+        if(control.roll < ROLLMIN){control.roll = ROLLMIN;}
         break;
         case(1):
-        control.roll--;
-        if(control.roll < ROLLMIN){control.roll = ROLLMIN;}
         break;
         case(2):
         control.roll++;
@@ -160,6 +161,7 @@
     }
 } 
 
+//Allowing intialization in various modes
 void initializeMode(){
     rosFlag = 0;
     if(abs(control.height - currentPosition) < 5){