File content as of revision 13:8630f38f8066:

 #include <std_msgs/Empty.h>
#include <std_msgs/Int32MultiArray.h>
#include <initializations.h>
#include <ros.h>
#include <definitions.h>
#include <prototypes.h>
#include "mbed.h" 

// Initializations

void setupGimbal(){
    // Setting Servo Modes
    gimbal.setMode(YAWID,SERVO,YAWMIN,YAWMAX);
    wait(0.1);
    gimbal.setMode(PITCHID,SERVO,PITCHMIN,PITCHMAX);
    wait(0.1);
    gimbal.setMode(ROLLID,SERVO,ROLLMIN,ROLLMAX);
    wait(0.1);   
    // Set the Gimbal at the zero position.
    gimbal.servo(YAWID, YAWZERO, 50); 
    control.yaw = YAWZERO;
    control.yawSpeed = 50;
    rosInput.yaw = YAWZERO;
    rosInput.yawSpeed = 50;
    wait(0.1);
    gimbal.servo(PITCHID, PITCHZERO, 50);
    control.pitch = PITCHZERO;
    control.pitchSpeed = 50;
    rosInput.pitch = PITCHZERO;
    rosInput.pitchSpeed = 50;
    wait(0.1);
    gimbal.servo(ROLLID, ROLLZERO, 50);
    control.roll = ROLLZERO;
    control.rollSpeed = 50;
    rosInput.roll = ROLLZERO;
    rosInput.rollSpeed = 50;
}

void setupLift(){
    
    startHallInt();
    liftSpeed.period_us(50);
    
    // Find the bottom position. Go down at lowest speed and set zero point.
    liftDirection.write(LIFTDOWN);
    liftSpeed.write(0.3);   
    
    
    // Wait until the motor stalls, know the motor is at bottom.
    while(stallcount < 3){wait(.01);}
    liftSpeed.write(0);
    currentPosition = 0;
    wait(0.1);
    liftDirection.write(LIFTUP);
    liftSpeed.write(0.3); 
    wait(0.1);
    liftSpeed.write(0);
    stopHallInt();
    rosInput.height = 1;
//    rosInput.height = LIFTHEIGHTMIN;
//    rosInput.heightSpeed = 0;
}

void setupROSNode(ros::NodeHandle& nh, ros::Subscriber<std_msgs::Int32MultiArray>& sub){     
    nh.getHardware()->setBaud(57600);
    nh.initNode();
    nh.subscribe(sub);
}

void startHallInt() { // Starting hall effect timer interrupt
    hallInt.attach(&hallInterrupt, 0.03);
}

void stopHallInt() { // Stopping hall effect timer interrupt
    hallInt.detach();
}