Diff: ros_functions.cpp

Revision:

13:8630f38f8066

Parent:

11:f99511d770ed

Child:

14:97804177806d

--- a/ros_functions.cpp	Thu Dec 13 18:25:16 2018 +0000
+++ b/ros_functions.cpp	Fri Dec 14 03:28:55 2018 +0000
@@ -26,11 +26,16 @@
 
 void rosCheck(){ 
         if(rosInput.mode == 0){ //Transport Mode
+            gimbal.servo(YAWID, YAWZERO, 50); 
             control.yaw = YAWZERO;
+            wait(0.1); 
+            gimbal.servo(PITCHID, PITCHZERO, 50);
+            wait(0.1); 
             control.pitch = PITCHZERO;
+            gimbal.servo(ROLLID, ROLLZERO, 50);
             control.roll = ROLLZERO;
             control.height = LIFTHEIGHTMIN;
-            control.heightSpeed = 0;
+            //control.heightSpeed = 0;
             if(initialize != rosInput.mode){
                 initializeMode();
             }
@@ -63,48 +68,58 @@
 }
 
 // Converting the ros inputs for the lift and gimbal and determining action
-void rosTranslator(int height, int yaw, int pitch, int roll){
+void rosTranslator(void){
     if(eStopFlag == 0){
         //Updating Height Position
-        if(rosInput.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
-        else if(rosInput.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
-        else {control.height = rosInput.height;}
-        //Updating Height Speed
-        switch(rosInput.heightSpeed){
+        switch(rosInput.height){
             case(0):
-            liftDutyCycle = 0;
+            control.height--;   
+            if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
             break;
             case(1):
-            liftDutyCycle = 0.1;
+            control.height = currentPosition;
             break;
             case(2):
-            liftDutyCycle = 0.2;
-            break;
-            case(3):
-            liftDutyCycle = 0.3;
-            break;
-            case(4):
-            liftDutyCycle = 0.4;
-            break;
-            case(5):
-            liftDutyCycle = 0.5;
-            break;
-            case(6):
-            liftDutyCycle = 0.6;
-            break;
-            case(7):
-            liftDutyCycle = 0.7;
-            break;
-            case(8):
-            liftDutyCycle = 0.8;
-            break;
-            case(9):
-            liftDutyCycle = 0.9;
-            break;
-            case(10):
-            liftDutyCycle = 1;
-            break;     
-        }
+            control.height++;  
+            if(control.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
+            break; 
+        } 
+        //Updating Height Speed
+        //switch(rosInput.heightSpeed){
+//            case(0):
+//            liftDutyCycle = 0;
+//            break;
+//            case(1):
+//            liftDutyCycle = 0.1;
+//            break;
+//            case(2):
+//            liftDutyCycle = 0.2;
+//            break;
+//            case(3):
+//            liftDutyCycle = 0.3;
+//            break;
+//            case(4):
+//            liftDutyCycle = 0.4;
+//            break;
+//            case(5):
+//            liftDutyCycle = 0.5;
+//            break;
+//            case(6):
+//            liftDutyCycle = 0.6;
+//            break;
+//            case(7):
+//            liftDutyCycle = 0.7;
+//            break;
+//            case(8):
+//            liftDutyCycle = 0.8;
+//            break;
+//            case(9):
+//            liftDutyCycle = 0.9;
+//            break;
+//            case(10):
+//            liftDutyCycle = 1;
+//            break;     
+//        }
 
         //Updating Yaw Position 
         if(rosInput.pitchSpeed == 0){}