File content as of revision 8:478f75c6109c:

 /*
 * Code for the Module Microcontroller as part of the 
 * Autonomous Camera Kart project.
 * Controls the Gimbal motors, raise and lower mechanism,
 * IMU sensor input, communication with the NUC onboard computer.
 */

// Library Includes
#include <stdint.h>
#include "mbed.h"
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Int32MultiArray.h>
#include <BNO055.h>
#include <initializations.h>
#include <definitions.h>
#include <prototypes.h>
#include <Mx28.h>

// Micro Module Declarations
PwmOut              liftSpeed(D10); // Normally D10
DigitalOut          liftDirection(D11);
DigitalIn           button(USER_BUTTON);
Ticker              hallInt;
Timer               filter_hall1;
Timer               rosMode_Delay;
DynamixelClass      gimbal(57600, D7, D8, D2);

ros::NodeHandle     nh;
ros::Subscriber<std_msgs::Int32MultiArray> sub("module_command", &module_commandCB);

// Global Variable
struct          fields control;
struct          fields rosInput;
volatile int    currentPosition = 0;
int             prevPosition = 0;
int             stallcount = 0;
bool            rosFlag = 1;

// Main Program
int main() {
    // Initializations
    filter_hall1.start();                       //Start Filtering Timer  
    setupGimbal();                              // Gimbal
    setupLift();                                // Lift
    setupROSNode(nh, sub);                     // ROS Node Handle
    rosInput.yaw = 1;
    rosInput.pitch = 1;
    rosInput.roll = 1;
    rosInput.height = 1;
    rosInput.mode = 0;

    // Main Loop
    while (1) {     
    // Lift Operation
        runLift(); 
    //Run Gimbal 
        runGimbal();
    //ROS Functions 
        nh.spinOnce();
        rosCheck();
    }    
}