File content as of revision 11:f99511d770ed:

#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
            control.yaw = YAWZERO;
            control.pitch = PITCHZERO;
            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(int height, int yaw, int pitch, int roll){
    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){
            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();
    }  
}