File content as of revision 4:89ebfa37663b:

#include <ros_functions.h>
#include <definitions.h>
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float32MultiArray.h>
#include <prototypes.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
int rosModeDelay = 1; //The amount of delay before switching modes 

void module_commandCB(const std_msgs::Float32MultiArray& command_data){
    rosInput.yaw = command_data.data[ROSYAW];
    rosInput.pitch = command_data.data[ROSPITCH];
    rosInput.roll = command_data.data[ROSROLL];
    rosInput.height = command_data.data[ROSHEIGHT];
    if(rosFlag == 1){
        rosInput.mode = command_data.data[ROSMODE];
    }
}

void rosCheck(){ 
        if(rosInput.mode == 0){ //Transport Mode
            if(initialize != rosInput.mode){
                control.yaw = YAWZERO;
                control.pitch = PITCHZERO;
                control.roll = ROLLZERO;
                control.height = LIFTHEIGHTMIN;
                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;
                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();
            } 
        }
        if(rosInput.mode == 2){ //Chat Mode - in bed
            if(initialize != rosInput.mode){
                control.pitch = PITCHMAX; // need to find values
                control.height = 1700; // need to find values
                control.yaw  = 2100; // need to find values
                initializeMode();
            }
            else{
                rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
                if(rosMode_Delay.read() > rosModeDelay){
                    rosFlag = 1;
                    rosMode_Delay.reset();
                    rosMode_Delay.stop();
                } 
            }
        }
        if(rosInput.mode == 3){ //Chat Mode - Sitting
            if(initialize != rosInput.mode){
                control.pitch = PITCHZERO;
                control.height = 1100; // need to find values
                control.yaw  = YAWZERO;
                control.roll  = ROLLZERO;
                initializeMode();
            }
            else{
                rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
                if(rosMode_Delay.read() > rosModeDelay){
                    rosFlag = 1;
                    rosMode_Delay.reset();
                    rosMode_Delay.stop();
                } 
            }
        }
        if(rosInput.mode == 4){ //Chat Mode - Standing
            if(initialize != rosInput.mode){
                control.pitch = 2122;
                control.height = LIFTHEIGHTMAX; 
                control.yaw  = YAWZERO;
                control.roll  = ROLLZERO;
                initializeMode();
            }
            else{
                rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
                if(rosMode_Delay.read() > rosModeDelay){
                    rosFlag = 1;
                    rosMode_Delay.reset();
                    rosMode_Delay.stop();
                } 
            }
        }  
}

void rosTranslator(int height, int yaw, int pitch, int roll){
    switch(height){
        case(0):
        control.height = currentPosition;
        break;
        case(1):
        control.height--;   
        if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
        break;
        case(2):
        control.height++;  
        if(control.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
        break; 
    } 
    switch(yaw){
        case(0):
        break;
        case(1):
        control.yaw++;
        if(control.yaw > YAWMAX){control.yaw = YAWMAX;}
        break;
        case(2):
        control.yaw--;
        if(control.yaw < YAWMIN){control.yaw = YAWMIN;}
        break;
    }
        switch(pitch){
        case(0):
        break; 
        case(1):
        control.pitch--;
        if(control.pitch < PITCHMIN){control.pitch = PITCHMIN;}
        break;
        case(2):
        control.pitch++;
        if(control.pitch > PITCHMAX){
        control.pitch = PITCHMAX;}
        break;
    }
    switch(rosInput.roll){
        case(0):
        break;
        case(1):
        control.roll--;
        if(control.roll < ROLLMIN){control.roll = ROLLMIN;}
        break;
        case(2):
        control.roll++;
        if(control.roll > ROLLMAX){control.roll = ROLLMAX;}
        break; 
    }
} 

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