File content as of revision 0:b1f7722c376d:

///////////////////////////////////////////////////////////////////// Beginning of code///////////////////////////////////////////////////////////////////
#include "mbed.h"
#include "ros.h" // Include ros package
#include "std_msgs/String.h" // Include String msg from std_msgs package
#include "std_msgs/Int32.h" // Include Int32 msg from std_msgs package

DigitalOut myled(LED1); // Use led to toggle to make sure code is running
//Serial pc(SERIAL_TX, SERIAL_RX); // set-up serial to pc
I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE
#define addr    (0x52) // I²C address of VL6180 shifted by 1 bit
                        //(0x29 << 1) so the R/W command can be added
///////////////////////////////////////////////////////////////////// Split 16-bit register address into two bytes and write // the address + data via I²C///////////////////////////////////////////////////////////////////
void WriteByte(wchar_t reg,char data){     
    char data_write[3];    
    data_write[0]=(reg >> 8)& 0xFF;; // MSB of register address    
    data_write[1]= reg  & 0xFF; // LSB of register address    
    data_write[2]= data & 0xFF;    
    i2c.write(addr, data_write, 3);
}

///////////////////////////////////////////////////////////////////// Split 16-bit register address into two bytes and write // required register address to VL6180 and read the data back///////////////////////////////////////////////////////////////////
char ReadByte(wchar_t reg){    
    char data_write[2];    
    char data_read[1];    
    data_write[0]=(reg >> 8)& 0xFF; // MSB of register address    
    data_write[1]= reg  & 0xFF; // LSB of register address    
    i2c.write(addr, data_write, 2);    
    i2c.read(addr, data_read, 1);
    return data_read[0];
}

///////////////////////////////////////////////////////////////////
// load settings
///////////////////////////////////////////////////////////////////
int VL6180_Init(){
    char reset;
    reset = ReadByte(0x016);
    if(reset==1){      // check to see has it be Initialised already
        ///////////////////////////////////////////////////////////////////
        // Mandatory : private registers
        WriteByte(0x0207, 0x01);
        WriteByte(0x0208, 0x01);
        WriteByte(0x0096, 0x00);
        WriteByte(0x0097, 0xfd);
        WriteByte(0x00e3, 0x01);
        WriteByte(0x00e4, 0x03);
        WriteByte(0x00e5, 0x02);
        WriteByte(0x00e6, 0x01);
        WriteByte(0x00e7, 0x03);
        WriteByte(0x00f5, 0x02);
        WriteByte(0x00d9, 0x05);
        WriteByte(0x00db, 0xce);
        WriteByte(0x00dc, 0x03);
        WriteByte(0x00dd, 0xf8);
        WriteByte(0x009f, 0x00);
        WriteByte(0x00a3, 0x3c);
        WriteByte(0x00b7, 0x00);
        WriteByte(0x00bb, 0x3c);
        WriteByte(0x00b2, 0x09);
        WriteByte(0x00ca, 0x09);
        WriteByte(0x0198, 0x01);
        WriteByte(0x01b0, 0x17);
        WriteByte(0x01ad, 0x00);
        WriteByte(0x00ff, 0x05);
        WriteByte(0x0100, 0x05);
        WriteByte(0x0199, 0x05);
        WriteByte(0x01a6, 0x1b);
        WriteByte(0x01ac, 0x3e);
        WriteByte(0x01a7, 0x1f);
        WriteByte(0x0030, 0x00);// Recommended : Public registers - See data sheet for more detail
        WriteByte(0x0011, 0x10); // Enables polling for ‘New Sample ready’  
        // when measurement completes
        WriteByte(0x010a, 0x30); // Set the averaging sample period 
        // (compromise between lower noise and  
        // increased execution time)
        WriteByte(0x003f, 0x46); // Sets the light and dark gain (upper  
        // nibble). Dark gain should not be                         
        //    changed.
        WriteByte(0x0031, 0xFF); // sets the # of range measurements after  
        // which auto calibration of system is 
        // performed 
        WriteByte(0x0041, 0x63); // Set ALS integration time to 100ms
        ///////////////////////////////////////////////////////////////////
            WriteByte(0x016, 0x00); //change fresh out of set status to 0
    }
    return 0;
}
///////////////////////////////////////////////////////////////////
// Start a range measurement in single shot mode
///////////////////////////////////////////////////////////////////
int VL6180_Start_Range(){
    WriteByte(0x018,0x01);
    return 0;
}

///////////////////////////////////////////////////////////////////
// poll for new sample ready ready
///////////////////////////////////////////////////////////////////
int VL6180_Poll_Range(){
    char status;
    char range_status;
    // check the status
    status = ReadByte(0x04f);
    range_status = status & 0x07;
    
    // wait for new measurement ready status   
    while(range_status !=  0x00){
        status = ReadByte(0x04f);
        range_status = status & 0x07;
        wait_ms(1); // (can be removed)
    }
    return 0;
}

///////////////////////////////////////////////////////////////////    
// Read range result (mm)
///////////////////////////////////////////////////////////////////
int VL6180_Read_Range(){
    int range;
    range=ReadByte(0x062);
    return range;
}
///////////////////////////////////////////////////////////////////
// clear interrupts
///////////////////////////////////////////////////////////////////
int VL6180_Clear_Interrupts(){
    WriteByte(0x015,0x07);
    return 0;
}
///////////////////////////////////////////////////////////////////
//  Main Program loop
///////////////////////////////////////////////////////////////////
int main()
{
    //Initialise node handler
    ros::NodeHandle nh;
    nh.initNode(); //Initialise rosnode
    
    //Initialises chatter rostopic publisher
    std_msgs::String str_msg; // Declares string msg entity
    ros::Publisher chatter("chatter", &str_msg); // Declares publisher for rostopic
    nh.advertise(chatter); // advertise publisher on ros server
    
    char hello[13] = "hello world!";
    
    //Initialises range rostopic publisher
    std_msgs::Int32 int_msg; // Declares integer 32 msg entity
    ros::Publisher range_pub("range", &int_msg); // Declares publisher for rostopic
    nh.advertise(range_pub); // advertises publisher on ros server
    
    int32_t range;
    myled = 0;
    //pc.baud(57600);
    // load settings onto VL6180X
    VL6180_Init();
    
    while (1) {
        myled = !myled;
        // start single range measurement
        VL6180_Start_Range();
        
        // poll the VL6180 till new sample ready
        VL6180_Poll_Range();
        
        // read range result
        range = VL6180_Read_Range();
        
        //clear the interrupt on VL6180
        VL6180_Clear_Interrupts();
        
        //send range to pc by serial
        //pc.printf("%d \r\n", range);
        
        str_msg.data = hello; // add hello to data member of string msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/String.html
        int_msg.data = range; // add range to data member of integer msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/Int32.html
        
        chatter.publish(&str_msg); // publish chatter rostopic
        range_pub.publish(&int_msg); // publish range rostopic
        
        nh.spinOnce(); // checks to see if ros master is running okay 
                        // If ros master isn't working then the code will exit the while loop
        wait(0.5);
    }
    
}