File content as of revision 2:48a9c25e5d26:

#include "mbed.h"
#include "Servo.h"
#include <ros.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Bool.h>

//------------Declare Global Variables------------------//

//      (m1)         (m2)
//          |       |
//          |       |
// (Ltrhust)|       |(Rthrust)
//          |       |
//          |       |    
//      (m3)         (m4)   

// Initialize PWM objects
PwmOut      m1(D8);
PwmOut      m2(D9);
PwmOut      m3(D10);
PwmOut      m4(D11);
PwmOut      lthrust(D12);
PwmOut      rthrust(D14);

int PWMBASELINE = 1500;
// Initialize all pwm values to the baseline value
int m1pwm = PWMBASELINE;
int m2pwm = PWMBASELINE;
int m3pwm = PWMBASELINE;
int m4pwm = PWMBASELINE;

int lthrustpwm = PWMBASELINE;
int rthrustpwm = PWMBASELINE;

int lfeedback  = 0;
int rfeedback  = 0;
int lthrust_tot = 0;
int rthrust_tot = 0;

int depthfeedback = 0;
int depthtot = 0;

// Tx - 11, Rx - 9
RawSerial device(PA_15, PB_7);
DigitalOut testLed(LED1);
Servo myservo(D3);
double pan_servo = 0;

//--------------ROS Node Configuration---------------
ros::NodeHandle nh;

std_msgs::Int16 depth_msg;
int depth = 0;
ros::Publisher depth_publisher("depth", &depth_msg);

void depth_motor_callback( const std_msgs::Int16& msg)
{  
    if (msg.data == 0)
    {
        m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE;   
    }
    else if (msg.data < 0)   
    {
        m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE - 25 + msg.data;
    }
    else{
        // Update motor pwm
        m1pwm = PWMBASELINE + 25 + msg.data;
        m2pwm = PWMBASELINE + 25 + msg.data;
        m3pwm = PWMBASELINE + 25 + msg.data;
        m4pwm = PWMBASELINE + 25 + msg.data;
    }
}

// Forward thrust callback returns valules [0:150]
void thrust_motor_callback( const std_msgs::Int16& msg)
{
    // Updates pwm values for forward thrust motors
    if (msg.data == 0)
    {
        // Stop motors
        lthrustpwm = rthrustpwm = PWMBASELINE;   
    }
    else if (msg.data < 0)
    {
        // Go reverse
        lthrustpwm = PWMBASELINE - 25 + msg.data;
        rthrustpwm = PWMBASELINE - 25 + msg.data;
    }
    else{
        // Go forward
        lthrustpwm = PWMBASELINE + 25 + msg.data;
        rthrustpwm = PWMBASELINE + 25 + msg.data;
    }
}

// Receives values ranging [0:75]
void yaw_feedback_callback( const std_msgs::Int16& msg)
{
    if (lthrustpwm == PWMBASELINE){
        if (msg.data > 0.25){
            lfeedback = (msg.data + 29) * -1;
            rfeedback = (msg.data + 29);
        }
        else if (msg.data < -0.25){
            lfeedback = (msg.data - 29) * -1;
            rfeedback = (msg.data - 29);   
        }
        else{
            lfeedback = rfeedback = 0;   
        }
    }
    else{   
        lfeedback = msg.data * -1;
        rfeedback = msg.data;       
    }
}   

// Receives depth feedback vallues from [0:50]
void depth_feedback_callback( const std_msgs::Int16& msg)
{
    // depth feedback control
    if (msg.data > 1)
    {
        depthfeedback = 25 + msg.data;
    }
    else if (msg.data < -1)
    {
        depthfeedback = -25 + msg.data;   
    }
    else{
        depthfeedback = 0;
    }   
}

void pan_callback( const std_msgs::Int16& msg)
{
    pan_servo = msg.data/100.0;
}

ros::Subscriber<std_msgs::Int16> depth_pwm_subscriber("depth_pwm", depth_motor_callback);
ros::Subscriber<std_msgs::Int16> yaw_pwm_subscriber("yaw_pwm", thrust_motor_callback);
ros::Subscriber<std_msgs::Int16> fb_yaw_subscriber("yaw_pwm_feedback", yaw_feedback_callback);
ros::Subscriber<std_msgs::Int16> fb_depth_subscriber("depth_pwm_feedback", depth_feedback_callback);
ros::Subscriber<std_msgs::Int16> pan_servo_subscriber("pan_servo", pan_callback);

int main() 
{    
    nh.initNode();
    nh.advertise(depth_publisher);
    nh.subscribe(depth_pwm_subscriber);
    nh.subscribe(yaw_pwm_subscriber);
    nh.subscribe(fb_yaw_subscriber);
    nh.subscribe(fb_depth_subscriber);
    nh.subscribe(pan_servo_subscriber);

    testLed = 0;
    
    while(1) 
    {       
        // If data is recieved, blink led
        if (device.readable())
        {
            // Obtain depth 
            depth = device.getc();
            // toggle led on
            testLed = 1;
        }
        
        // Publish depth information
        depth_msg.data = depth;
        depth_publisher.publish(&depth_msg);
        
        // Depth thrust + feedback
        depthtot = m1pwm + depthfeedback;
        
        // Output motor pwm
        m1.pulsewidth_us(depthtot);
        m2.pulsewidth_us(depthtot);
        m3.pulsewidth_us(depthtot);
        m4.pulsewidth_us(depthtot);
        
        // Fwd thrust + feedback
        lthrust_tot = lthrustpwm + lfeedback;
        rthrust_tot = rthrustpwm + rfeedback;
        
        // Output pwm to fwd/rev thrusters    
        lthrust.pulsewidth_us(lthrust_tot);
        rthrust.pulsewidth_us(rthrust_tot);
        
        // actuate pan tilt camera
        myservo = pan_servo;
        
        nh.spinOnce();
        wait_ms(5);
        testLed = 0;
    }
}