Light sensor for ROS on Nucleo board

Dependencies:   mbed ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /*
00002  * rosserial Publisher Example
00003  * Prints "hello world!"
00004  */
00005 
00006 #include"mbed.h"
00007 #include <ros.h>
00008 #include <std_msgs/String.h>
00009 #include <std_msgs/Int32.h>
00010 
00011 DigitalOut led = LED1;
00012 DigitalOut SHDN_1(D0);
00013 
00014 // set-up serial to pc 
00015 I2C i2c(I2C_SDA, I2C_SCL); 
00016 // Set up I²C on the STM32 NUCLEO-401RE 
00017 #define addr1    (0x52) 
00018 #define addr2    (0x60)  
00019 // I²C address of VL6180 shifted by 1 bit                        
00020 //(0x29 << 1) so the R/W command can be added
00021 
00022 /////////////////////////////////////////////////////////////////// 
00023 // Split 16-bit register address into two bytes and write 
00024 // the address + data via I²C 
00025 /////////////////////////////////////////////////////////////////// 
00026 void WriteByte(wchar_t reg,char data, char addr) {     
00027     char data_write[3];
00028     data_write[0] = (reg >> 8) & 0xFF;; 
00029     // MSB of register address    
00030     data_write[1] = reg  & 0xFF; 
00031     // LSB of register address    
00032     data_write[2] = data & 0xFF;
00033     i2c.write(addr, data_write, 3); 
00034 }
00035 
00036 /////////////////////////////////////////////////////////////////// 
00037 // Split 16-bit register address into two bytes and write 
00038 // required register address to VL6180 and read the data back 
00039 /////////////////////////////////////////////////////////////////// 
00040 char ReadByte(wchar_t reg, char addr) {    
00041     char data_write[2];    
00042     char data_read[1];
00043     
00044     data_write[0] = (reg >> 8) & 0xFF; // MSB of register address    
00045     data_write[1] = reg  & 0xFF; // LSB of register address
00046     
00047     i2c.write(addr, data_write, 2);    
00048     i2c.read(addr, data_read, 1);    
00049     return data_read[0]; 
00050 }
00051     
00052 /////////////////////////////////////////////////////////////////// 
00053 // load settings 
00054 /////////////////////////////////////////////////////////////////// 
00055 int VL6180_Init(char addr) {    
00056     char reset;
00057 
00058     //WriteByte(0x212, addr/2, 0x52);
00059     
00060     
00061     reset = ReadByte(0x016, addr);    
00062     if (reset==1)
00063     {      
00064         // check to see has it be Initialised already
00065     
00066         /////////////////////////////////////////////////////////////////// 
00067         // SENSOR 1
00068         ///////////////////////////////////////////////////////////////////
00069         // Mandatory : private registers 
00070         WriteByte(0x0207, 0x01, addr); 
00071         WriteByte(0x0208, 0x01, addr); 
00072         WriteByte(0x0096, 0x00, addr); 
00073         WriteByte(0x0097, 0xfd, addr); 
00074         WriteByte(0x00e3, 0x01, addr); 
00075         WriteByte(0x00e4, 0x03, addr); 
00076         WriteByte(0x00e5, 0x02, addr); 
00077         WriteByte(0x00e6, 0x01, addr); 
00078         WriteByte(0x00e7, 0x03, addr); 
00079         WriteByte(0x00f5, 0x02, addr); 
00080         WriteByte(0x00d9, 0x05, addr); 
00081         WriteByte(0x00db, 0xce, addr); 
00082         WriteByte(0x00dc, 0x03, addr); 
00083         WriteByte(0x00dd, 0xf8, addr); 
00084         WriteByte(0x009f, 0x00, addr); 
00085         WriteByte(0x00a3, 0x3c, addr); 
00086         WriteByte(0x00b7, 0x00, addr); 
00087         WriteByte(0x00bb, 0x3c, addr); 
00088         WriteByte(0x00b2, 0x09, addr); 
00089         WriteByte(0x00ca, 0x09, addr); 
00090         WriteByte(0x0198, 0x01, addr); 
00091         WriteByte(0x01b0, 0x17, addr); 
00092         WriteByte(0x01ad, 0x00, addr); 
00093         WriteByte(0x00ff, 0x05, addr); 
00094         WriteByte(0x0100, 0x05, addr); 
00095         WriteByte(0x0199, 0x05, addr); 
00096         WriteByte(0x01a6, 0x1b, addr); 
00097         WriteByte(0x01ac, 0x3e, addr); 
00098         WriteByte(0x01a7, 0x1f, addr); 
00099         WriteByte(0x0030, 0x00, addr);
00100         
00101         WriteByte(0x016, 0x00, addr); //change fresh out of set status to 0    
00102     }    
00103     return 0; 
00104 }
00105 
00106 /////////////////////////////////////////////////////////////////// 
00107 // Start a range measurement in single shot mode 
00108 /////////////////////////////////////////////////////////////////// 
00109 int VL6180_Start_Range(char addr) {    
00110     WriteByte(0x018,0x03, addr);  
00111     return 0;
00112 }
00113 
00114 /////////////////////////////////////////////////////////////////// 
00115 // poll for new sample ready ready 
00116 /////////////////////////////////////////////////////////////////// 
00117 
00118 int VL6180_Poll_Range(char addr) {    
00119     char status;    
00120     char range_status;  
00121           
00122     // check the status    
00123     status = ReadByte(0x04f,addr);            
00124     range_status = status & 0x07; 
00125            
00126     // wait for new measurement ready status    
00127     while (range_status !=  0x00) 
00128     {         
00129         status = ReadByte(0x04f,addr);        
00130         range_status = status & 0x07;        
00131         wait_ms(1); // (can be removed)        
00132     }
00133     
00134     return 0;     
00135 }
00136 
00137 
00138 /////////////////////////////////////////////////////////////////// 
00139 // Read range result (mm) 
00140 /////////////////////////////////////////////////////////////////// 
00141 int VL6180_Read_Range(char addr) {    
00142     int range;    
00143     range=ReadByte(0x062, addr);    
00144     return range; 
00145 } 
00146 
00147 /////////////////////////////////////////////////////////////////// 
00148 // clear interrupts 
00149 /////////////////////////////////////////////////////////////////// 
00150 int VL6180_Clear_Interrupts() {    
00151     WriteByte(0x015,0x07, addr1);    
00152     WriteByte(0x015,0x07, addr2);  
00153     return 0; 
00154 }
00155 
00156 /////////////////////////////////////////////////////////////////// 
00157 //  Main Program loop 
00158 /////////////////////////////////////////////////////////////////// 
00159 int main() {   
00160  
00161     ros::NodeHandle  nh;
00162     nh.initNode();
00163     
00164     std_msgs::Int32 int_sensor1_msg;
00165     std_msgs::Int32 int_sensor2_msg;
00166     ros::Publisher range1_pub("sensor1", &int_sensor1_msg);
00167     ros::Publisher range2_pub("sensor2", &int_sensor2_msg);
00168 
00169     nh.advertise(range1_pub);
00170     nh.advertise(range2_pub);
00171     
00172     int range1; 
00173     int range2;
00174     
00175     SHDN_1 = 0;
00176     
00177     //change default address of sensor 2
00178     WriteByte(0x212, 0x30, addr1);
00179     
00180     SHDN_1 = 1;
00181     
00182     //SHDN_1 = 1;
00183     // load settings onto VL6180X    
00184     VL6180_Init(addr1);
00185     //SHDN_1 = 0; 
00186     VL6180_Init(addr2);
00187   
00188     while (1)
00189     {            
00190         // start range measurement        
00191         
00192         // poll the VL6180 till new sample ready    
00193         VL6180_Start_Range(addr1);    
00194         VL6180_Poll_Range(addr1);   
00195         range1 = VL6180_Read_Range(addr1);
00196                   
00197         // read range result 
00198         VL6180_Start_Range(addr2);         
00199         VL6180_Poll_Range(addr2); 
00200         range2 = VL6180_Read_Range(addr2);                   
00201         // clear the interrupt on VL6180        
00202         VL6180_Clear_Interrupts();                 
00203         // send range to pc by serial
00204          
00205         int_sensor1_msg.data = range1;
00206         int_sensor2_msg.data = range2;    
00207         range1_pub.publish(&int_sensor1_msg);     
00208         range2_pub.publish(&int_sensor2_msg);  
00209         nh.spinOnce();      
00210         wait(0.1); 
00211         
00212     } 
00213 } 
00214