File content as of revision 13:8630f38f8066:

#include <ros_functions.h>
#include <definitions.h>
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Int32MultiArray.h>
#include <prototypes.h>

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){
    rosInput.yaw = command_data.data[ROSYAW];
    rosInput.pitch = command_data.data[ROSPITCH];
    rosInput.roll = command_data.data[ROSROLL];
    rosInput.yawSpeed = command_data.data[ROSYAWSP]; 
    rosInput.height = command_data.data[ROSHEIGHT];
    rosInput.rollSpeed = command_data.data[ROSROLLSP]; 
    rosInput.pitchSpeed = command_data.data[ROSPITCHSP]; 
    rosInput.heightSpeed = command_data.data[ROSHEIGHTSP]; 
    if(rosFlag == 1){
        rosInput.mode = command_data.data[ROSMODE];
    }
}

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;
            if(initialize != rosInput.mode){
                initializeMode();
            }
            else{
                if(rosMode_Delay.read() > rosModeDelay){
                    rosFlag = 1;
                    rosMode_Delay.reset();
                    rosMode_Delay.stop();
                } 
            }     
        }
        else if(rosInput.mode == 1){ //Photo Mode        
            if(h != rosInput.height){
                control.height = currentPosition;
            }
            if(initialize != rosInput.mode){
                rosFlag = 0;
                control.height = currentPosition;
                initialize = rosInput.mode;
                rosMode_Delay.start(); 
            }
            rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
            h = rosInput.height; 
            if(rosMode_Delay.read() > rosModeDelay){
                rosFlag = 1;
                rosMode_Delay.reset();
                rosMode_Delay.stop();
            } 
        }
}

// Converting the ros inputs for the lift and gimbal and determining action
void rosTranslator(void){
    if(eStopFlag == 0){
        //Updating Height Position
        switch(rosInput.height){
            case(0):
            control.height--;   
            if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
            break;
            case(1):
            control.height = currentPosition;
            break;
            case(2):
            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){}
        else {
            if(rosInput.yaw < YAWMIN){control.yaw = YAWMIN;}
            else if(rosInput.yaw > YAWMAX){control.yaw = YAWMAX;}
            else {control.yaw = rosInput.yaw;}
        }
        //Updating Roll Speed
        if(rosInput.yawSpeed > SERVOSPEEDMAX){control.yawSpeed = SERVOSPEEDMAX;}
        else if(rosInput.yawSpeed < SERVOSPEEDMIN){control.yawSpeed = SERVOSPEEDMIN;}
        else {control.yawSpeed = rosInput.yawSpeed;}

        //Updating Pitch Position
        if(rosInput.pitchSpeed == 0){}
        else {
            if(rosInput.pitch < PITCHMIN){control.pitch = PITCHMIN;}
            else if(rosInput.pitch > PITCHMAX){control.pitch = PITCHMAX;}
            else {control.pitch = rosInput.pitch;}
        }
        //Updating Pitch Speed
        if(rosInput.pitchSpeed > SERVOSPEEDMAX){control.pitchSpeed = SERVOSPEEDMAX;}
        else if(rosInput.pitchSpeed < SERVOSPEEDMIN){control.pitchSpeed = SERVOSPEEDMIN;}
        else {control.pitchSpeed = rosInput.pitchSpeed;}

        //Updating Roll Position
        if(rosInput.rollSpeed == 0){}
        else {
            if(rosInput.roll < ROLLMIN){control.roll = ROLLMIN;}
            else if(rosInput.roll > ROLLMAX){control.roll = ROLLMAX;}
            else {control.roll = rosInput.roll;}
        }
        //Updating Roll Speed
        if(rosInput.rollSpeed > SERVOSPEEDMAX){control.rollSpeed = SERVOSPEEDMAX;}
        else if(rosInput.rollSpeed < SERVOSPEEDMIN){control.rollSpeed = SERVOSPEEDMIN;}
        else {control.rollSpeed = rosInput.rollSpeed;}
        
    }
} 

//Allowing intialization in various modes
void initializeMode(){
    rosFlag = 0;
    if(abs(control.height - currentPosition) < 5){
        initialize = rosInput.mode;
        rosMode_Delay.start();
    }  
}