ros_functions.cpp

00001 
00002 #include <ros_functions.h>
00003 #include <definitions.h>
00004 #include <ros.h>
00005 #include <std_msgs/Empty.h>
00006 #include <std_msgs/Int32MultiArray.h>
00007 #include <prototypes.h>
00008 
00009 static int h = 0; //variable used to reset the currentPosition value when switching input direction
00010 static int initialize = 0; //Variable used to allow the set up of each mode
00011 int rosModeDelay = 1; //The amount of delay before switching modes 
00012 
00013 void module_commandCB(const std_msgs::Int32MultiArray& command_data){
00014     rosInput.yaw = command_data.data[ROSYAW];
00015     rosInput.pitch = command_data.data[ROSPITCH];
00016     rosInput.roll = command_data.data[ROSROLL];
00017     rosInput.yawSpeed = command_data.data[ROSYAWSP]; 
00018     rosInput.height = command_data.data[ROSHEIGHT];
00019     rosInput.rollSpeed = command_data.data[ROSROLLSP]; 
00020     rosInput.pitchSpeed = command_data.data[ROSPITCHSP]; 
00021     rosInput.heightSpeed = command_data.data[ROSHEIGHTSP]; 
00022     if(rosFlag == 1){
00023         rosInput.mode = command_data.data[ROSMODE];
00024     }
00025 }
00026 
00027 void rosCheck(){ 
00028         if(rosInput.mode == 0){ //Transport Mode
00029             gimbal.servo(YAWID, YAWZERO, 50); 
00030             control.yaw = YAWZERO;
00031             wait(0.1); 
00032             gimbal.servo(PITCHID, PITCHZERO, 50);
00033             wait(0.1); 
00034             control.pitch = PITCHZERO;
00035             gimbal.servo(ROLLID, ROLLZERO, 50);
00036             control.roll = ROLLZERO;
00037             control.height = LIFTHEIGHTMIN;
00038             //control.heightSpeed = 0;
00039             if(initialize != rosInput.mode){
00040                 initializeMode();
00041             }
00042             else{
00043                 if(rosMode_Delay.read() > rosModeDelay){
00044                     rosFlag = 1;
00045                     rosMode_Delay.reset();
00046                     rosMode_Delay.stop();
00047                 } 
00048             }     
00049         }
00050         else if(rosInput.mode == 1){ //Photo Mode        
00051             if(h != rosInput.height){
00052                 control.height = currentPosition;
00053             }
00054             if(initialize != rosInput.mode){
00055                 rosFlag = 0;
00056                 control.height = currentPosition;
00057                 initialize = rosInput.mode;
00058                 rosMode_Delay.start(); 
00059             }
00060             rosTranslator();
00061             h = rosInput.height; 
00062             if(rosMode_Delay.read() > rosModeDelay){
00063                 rosFlag = 1;
00064                 rosMode_Delay.reset();
00065                 rosMode_Delay.stop();
00066             } 
00067         }
00068 }
00069 
00070 // Converting the ros inputs for the lift and gimbal and determining action
00071 void rosTranslator(void){
00072     if(eStopFlag == 0){
00073         //Updating Height Position
00074         switch(rosInput.height){
00075             case(0):
00076             control.height--;   
00077             if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
00078             break;
00079             case(1):
00080             control.height = currentPosition;
00081             break;
00082             case(2):
00083             control.height++;  
00084             if(control.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
00085             break; 
00086         } 
00087         //Updating Height Speed
00088         //switch(rosInput.heightSpeed){
00089 //            case(0):
00090 //            liftDutyCycle = 0;
00091 //            break;
00092 //            case(1):
00093 //            liftDutyCycle = 0.1;
00094 //            break;
00095 //            case(2):
00096 //            liftDutyCycle = 0.2;
00097 //            break;
00098 //            case(3):
00099 //            liftDutyCycle = 0.3;
00100 //            break;
00101 //            case(4):
00102 //            liftDutyCycle = 0.4;
00103 //            break;
00104 //            case(5):
00105 //            liftDutyCycle = 0.5;
00106 //            break;
00107 //            case(6):
00108 //            liftDutyCycle = 0.6;
00109 //            break;
00110 //            case(7):
00111 //            liftDutyCycle = 0.7;
00112 //            break;
00113 //            case(8):
00114 //            liftDutyCycle = 0.8;
00115 //            break;
00116 //            case(9):
00117 //            liftDutyCycle = 0.9;
00118 //            break;
00119 //            case(10):
00120 //            liftDutyCycle = 1;
00121 //            break;     
00122 //        }
00123 
00124         //Updating Yaw Position 
00125         if(rosInput.pitchSpeed == 0){}
00126         else {
00127             if(rosInput.yaw < YAWMIN){control.yaw = YAWMIN;}
00128             else if(rosInput.yaw > YAWMAX){control.yaw = YAWMAX;}
00129             else {control.yaw = rosInput.yaw;}
00130         }
00131         //Updating Roll Speed
00132         if(rosInput.yawSpeed > SERVOSPEEDMAX){control.yawSpeed = SERVOSPEEDMAX;}
00133         else if(rosInput.yawSpeed < SERVOSPEEDMIN){control.yawSpeed = SERVOSPEEDMIN;}
00134         else {control.yawSpeed = rosInput.yawSpeed;}
00135 
00136         //Updating Pitch Position
00137         if(rosInput.pitchSpeed == 0){}
00138         else {
00139             if(rosInput.pitch < PITCHMIN){control.pitch = PITCHMIN;}
00140             else if(rosInput.pitch > PITCHMAX){control.pitch = PITCHMAX;}
00141             else {control.pitch = rosInput.pitch;}
00142         }
00143         //Updating Pitch Speed
00144         if(rosInput.pitchSpeed > SERVOSPEEDMAX){control.pitchSpeed = SERVOSPEEDMAX;}
00145         else if(rosInput.pitchSpeed < SERVOSPEEDMIN){control.pitchSpeed = SERVOSPEEDMIN;}
00146         else {control.pitchSpeed = rosInput.pitchSpeed;}
00147 
00148         //Updating Roll Position
00149         if(rosInput.rollSpeed == 0){}
00150         else {
00151             if(rosInput.roll < ROLLMIN){control.roll = ROLLMIN;}
00152             else if(rosInput.roll > ROLLMAX){control.roll = ROLLMAX;}
00153             else {control.roll = rosInput.roll;}
00154         }
00155         //Updating Roll Speed
00156         if(rosInput.rollSpeed > SERVOSPEEDMAX){control.rollSpeed = SERVOSPEEDMAX;}
00157         else if(rosInput.rollSpeed < SERVOSPEEDMIN){control.rollSpeed = SERVOSPEEDMIN;}
00158         else {control.rollSpeed = rosInput.rollSpeed;}
00159         
00160     }
00161 } 
00162 
00163 //Allowing intialization in various modes
00164 void initializeMode(){
00165     rosFlag = 0;
00166     if(abs(control.height - currentPosition) < 5){
00167         initialize = rosInput.mode;
00168         rosMode_Delay.start();
00169     }  
00170 }