File content as of revision 7:c5017af4c555:

#include"mbed.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>

Serial pc(SERIAL_TX, SERIAL_RX); // set-up serial to pc
DigitalOut led = LED1;
DigitalOut SHDN_1(PC_9);
DigitalOut SHDN_2(PC_11);
DigitalOut SHDN_3(PD_2);
DigitalOut SHDN_4(PG_3);


// set-up serial to pc 
I2C i2c(I2C_SDA, I2C_SCL); 
// Set up I²C on the STM32 NUCLEO-401RE 
#define addr1    (0x29) 
#define addr2    (0x2A)  
#define addr3   (0x2B)
#define addr4   (0x2C)

//Macro for addresses
#define ADDR1   (addr1<<1)
#define ADDR2   (addr2<<1)
#define ADDR3   (addr3<<1)
#define ADDR4   (addr4<<1)


// 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 addr) {     
    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 addr) {    
    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 addr) {    
    char reset;    
    
    reset = ReadByte(0x016, addr);    
    if (reset==1)
    {      
        // check to see has it be Initialised already
    
        /////////////////////////////////////////////////////////////////// 
        // SENSOR 1
        ///////////////////////////////////////////////////////////////////
        // Mandatory : private registers 
        WriteByte(0x0207, 0x01, addr); 
        WriteByte(0x0208, 0x01, addr); 
        WriteByte(0x0096, 0x00, addr); 
        WriteByte(0x0097, 0xfd, addr); 
        WriteByte(0x00e3, 0x01, addr); 
        WriteByte(0x00e4, 0x03, addr); 
        WriteByte(0x00e5, 0x02, addr); 
        WriteByte(0x00e6, 0x01, addr); 
        WriteByte(0x00e7, 0x03, addr); 
        WriteByte(0x00f5, 0x02, addr); 
        WriteByte(0x00d9, 0x05, addr); 
        WriteByte(0x00db, 0xce, addr); 
        WriteByte(0x00dc, 0x03, addr); 
        WriteByte(0x00dd, 0xf8, addr); 
        WriteByte(0x009f, 0x00, addr); 
        WriteByte(0x00a3, 0x3c, addr); 
        WriteByte(0x00b7, 0x00, addr); 
        WriteByte(0x00bb, 0x3c, addr); 
        WriteByte(0x00b2, 0x09, addr); 
        WriteByte(0x00ca, 0x09, addr); 
        WriteByte(0x0198, 0x01, addr); 
        WriteByte(0x01b0, 0x17, addr); 
        WriteByte(0x01ad, 0x00, addr); 
        WriteByte(0x00ff, 0x05, addr); 
        WriteByte(0x0100, 0x05, addr); 
        WriteByte(0x0199, 0x05, addr); 
        WriteByte(0x01a6, 0x1b, addr); 
        WriteByte(0x01ac, 0x3e, addr); 
        WriteByte(0x01a7, 0x1f, addr); 
        WriteByte(0x0030, 0x00, addr);
        
        WriteByte(0x016, 0x00, addr); //change fresh out of set status to 0    
    }    
    return 0; 
}

/////////////////////////////////////////////////////////////////// 
// Start a range measurement in single shot mode 
/////////////////////////////////////////////////////////////////// 
int VL6180_Start_Range(char addr) {    
    WriteByte(0x018,0x03, addr);  
    return 0;
}

/////////////////////////////////////////////////////////////////// 
// poll for new sample ready ready 
/////////////////////////////////////////////////////////////////// 

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


/////////////////////////////////////////////////////////////////// 
// Read range result (mm) 
/////////////////////////////////////////////////////////////////// 
int VL6180_Read_Range(char addr) {    
    int range;    
    range=ReadByte(0x062, addr);    
    return range; 
} 

/////////////////////////////////////////////////////////////////// 
// clear interrupts 
/////////////////////////////////////////////////////////////////// 
int VL6180_Clear_Interrupts(char addr) {    
    WriteByte(0x015,0x07, addr);    
  //  WriteByte(0x015,0x07, addr2);  
    return 0; 
}

/////////////////////////////////////////////////////////////////// 
//  Main Program loop 
/////////////////////////////////////////////////////////////////// 
int main() {  
    SHDN_1 = 0;
    SHDN_3 = 0;
    SHDN_2 = 0;
    SHDN_4 = 0;
    wait_ms(0.5);
 
    SHDN_1 = 0;
    SHDN_2 = 1;
    SHDN_3 = 0;
    SHDN_4 = 0;
//    ros::NodeHandle  nh;
//    nh.initNode();
//    
//    std_msgs::Int32 int_sensor1_msg;
//    std_msgs::Int32 int_sensor2_msg;
//    ros::Publisher range1_pub("sensor1", &int_sensor1_msg);

//
//    nh.advertise(range1_pub);
//    nh.advertise(range2_pub);
    
    int range1; 
    int range2;
    int range3;
    int range4;
    
    
    
    //change default address of sensor 4
    VL6180_Init(ADDR1);
    WriteByte(0x212, addr2, ADDR1);
    SHDN_3 = 1;
    VL6180_Init(ADDR1);
    WriteByte(0x212, addr3, ADDR1);
    SHDN_4 = 1;
    VL6180_Init(ADDR1);
    WriteByte(0x212, addr4, ADDR1);
    SHDN_1 = 1;
    VL6180_Init(ADDR1);
    // load settings onto VL6180X    
    //VL6180_Init(addr1<<1);
    //SHDN_1 = 0; 
    //VL6180_Init(addr2<<1);
    
  
    while (1)
    {            
        // start range measurement        
        
        // poll the VL6180 till new sample ready    
        VL6180_Start_Range(ADDR1);    
        VL6180_Poll_Range(ADDR1);   
        range1 = VL6180_Read_Range(ADDR1);
        VL6180_Clear_Interrupts(ADDR1);  
                 
        // read range result 
        VL6180_Start_Range(ADDR2);         
        VL6180_Poll_Range(ADDR2); 
        range2 = VL6180_Read_Range(ADDR2); 
        VL6180_Clear_Interrupts(ADDR2);    
        
        VL6180_Start_Range(ADDR3);         
        VL6180_Poll_Range(ADDR3); 
        range3 = VL6180_Read_Range(ADDR3);
        VL6180_Clear_Interrupts(ADDR3);  
        
        VL6180_Start_Range(ADDR4);         
        VL6180_Poll_Range(ADDR4); 
        range4 = VL6180_Read_Range(ADDR4);
        VL6180_Clear_Interrupts(ADDR4);                 
        // clear the interrupt on VL6180                 
        // send range to pc by serial
         
//        int_sensor1_msg.data = range1;
//        int_sensor2_msg.data = range2;    
//        range1_pub.publish(&int_sensor1_msg);     
//        range2_pub.publish(&int_sensor2_msg);  
//        nh.spinOnce();
       pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);  
       //pc.printf("lfajfl\r\n");
       //pc.printf("Range one = %d\r\n ",range1);      
        wait_ms(100); 
        
    } 
}