File content as of revision 0:659581d9d096:

#include "mbed.h"
#include "Servo.h"

#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt16.h>

 
// ===== LED
DigitalOut myled(LED2);
DigitalOut led = LED1;

// ===== SERVOS
Servo grab(D3);
Servo wrist(D6);
Servo dropl(A6); // not plugged in yet
Servo dropr(D10); // not plugged in yet
Servo flag(D9);

// ===== VARIABLES
DigitalIn carousel(D11, PullDown);
DigitalIn start(A5, PullDown);


// ===== FUNCTIONS
void grab_callback(const std_msgs::Float32& pos){
    grab = pos.data;
}

void wrist_callback(const std_msgs::Float32& pos){
    wrist = pos.data;
}

void dropl_callback(const std_msgs::Float32& pos){
    dropl = pos.data;
}

void dropr_callback(const std_msgs::Float32& pos){
    dropr = pos.data;
}

void flag_callback(const std_msgs::Float32& pos){
    flag = pos.data;
}


// ===== ROSSERIAL SETUP
ros::NodeHandle  nh;

// Subscribers
ros::Subscriber<std_msgs::Float32> grab_pos("hardware/servos/grab", grab_callback);
//ros::Subscriber<std_msgs::Float32> dropl_pos("hardware/servos/dropl", wirst_callback);
//ros::Subscriber<std_msgs::Float32> dropr_pos("hardware/servos/dropr", wirst_callback);
ros::Subscriber<std_msgs::Float32> wrist_pos("hardware/servos/wrist", wrist_callback);
ros::Subscriber<std_msgs::Float32> flag_pos("hardware/servos/flag", flag_callback);

// Publishers
std_msgs::UInt16 start_msg;
ros::Publisher start_pub("hardware/switches/start", &start_msg);

std_msgs::UInt16 carousel_msg;
ros::Publisher carousel_pub("hardware/switches/carousel", &carousel_msg);

// ===== MAIN
int main() {
    nh.initNode();
    
    nh.subscribe(grab_pos);
    nh.subscribe(wrist_pos);
    //nh.subscribe(dropr_pos);
    //nh.subscribe(dropl_pos);
    nh.subscribe(flag_pos);
    
    nh.advertise(start_pub);
    nh.advertise(carousel_pub);

    while (1) {
        led = !led; // Toggle LED
        
        start_msg.data = start;
        start_pub.publish( &start_msg );
        
        carousel_msg.data = carousel;
        carousel_pub.publish( &carousel_msg );
        
        // Spin the rosserial node handler
        nh.spinOnce();
        
        wait(0.02); // wait.....
    }
}