It is supposed to work

Dependencies:   mbed ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 
00002 #include"mbed.h"
00003 #include <ros.h>
00004 #include <std_msgs/String.h>
00005 #include <std_msgs/Int32.h>
00006 
00007 Serial pc(SERIAL_TX, SERIAL_RX); // set-up serial to pc
00008 DigitalOut led = LED1;
00009 DigitalOut SHDN_1(PC_9);
00010 DigitalOut SHDN_2(PC_11);
00011 DigitalOut SHDN_3(PD_2);
00012 DigitalOut SHDN_4(PG_3);
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   (0x29) 
00018 #define addr2   (0x2A)  
00019 #define addr3   (0x2B)
00020 #define addr4   (0x2C)
00021 
00022 //Macro for addresses
00023 #define ADDR1   (addr1<<1)
00024 #define ADDR2   (addr2<<1)
00025 #define ADDR3   (addr3<<1)
00026 #define ADDR4   (addr4<<1)
00027 // I²C address of VL6180 shifted by 1 bit                        
00028 //(0x29 << 1) so the R/W command can be added
00029 
00030 /////////////////////////////////////////////////////////////////// 
00031 // Split 16-bit register address into two bytes and write 
00032 // the address + data via I²C 
00033 /////////////////////////////////////////////////////////////////// 
00034 void WriteByte(wchar_t reg,char data, char addr) {     
00035     char data_write[3];
00036     data_write[0] = (reg >> 8) & 0xFF;; 
00037     // MSB of register address    
00038     data_write[1] = reg  & 0xFF; 
00039     // LSB of register address    
00040     data_write[2] = data & 0xFF;
00041     i2c.write(addr, data_write, 3); 
00042 }
00043 
00044 /////////////////////////////////////////////////////////////////// 
00045 // Split 16-bit register address into two bytes and write 
00046 // required register address to VL6180 and read the data back 
00047 /////////////////////////////////////////////////////////////////// 
00048 char ReadByte(wchar_t reg, char addr) {    
00049     char data_write[2];    
00050     char data_read[1];
00051     
00052     data_write[0] = (reg >> 8) & 0xFF; // MSB of register address    
00053     data_write[1] = reg  & 0xFF; // LSB of register address
00054     
00055     i2c.write(addr, data_write, 2);    
00056     i2c.read(addr, data_read, 1);    
00057     return data_read[0]; 
00058 }
00059     
00060 /////////////////////////////////////////////////////////////////// 
00061 // load settings 
00062 /////////////////////////////////////////////////////////////////// 
00063 int VL6180_Init(char addr) {    
00064     char reset;    
00065     
00066     // check to see has it be Initialised already
00067     reset = ReadByte(0x016, addr);    
00068     if (reset==1)
00069     {       
00070         /////////////////////////////////////////////////////////////////// 
00071         // DEFAULT SETTINGS
00072         ///////////////////////////////////////////////////////////////////
00073         // Mandatory : private registers 
00074         WriteByte(0x0207, 0x01, addr); 
00075         WriteByte(0x0208, 0x01, addr); 
00076         WriteByte(0x0096, 0x00, addr); 
00077         WriteByte(0x0097, 0xfd, addr); 
00078         WriteByte(0x00e3, 0x01, addr); 
00079         WriteByte(0x00e4, 0x03, addr); 
00080         WriteByte(0x00e5, 0x02, addr); 
00081         WriteByte(0x00e6, 0x01, addr); 
00082         WriteByte(0x00e7, 0x03, addr); 
00083         WriteByte(0x00f5, 0x02, addr); 
00084         WriteByte(0x00d9, 0x05, addr); 
00085         WriteByte(0x00db, 0xce, addr); 
00086         WriteByte(0x00dc, 0x03, addr); 
00087         WriteByte(0x00dd, 0xf8, addr); 
00088         WriteByte(0x009f, 0x00, addr); 
00089         WriteByte(0x00a3, 0x3c, addr); 
00090         WriteByte(0x00b7, 0x00, addr); 
00091         WriteByte(0x00bb, 0x3c, addr); 
00092         WriteByte(0x00b2, 0x09, addr); 
00093         WriteByte(0x00ca, 0x09, addr); 
00094         WriteByte(0x0198, 0x01, addr); 
00095         WriteByte(0x01b0, 0x17, addr); 
00096         WriteByte(0x01ad, 0x00, addr); 
00097         WriteByte(0x00ff, 0x05, addr); 
00098         WriteByte(0x0100, 0x05, addr); 
00099         WriteByte(0x0199, 0x05, addr); 
00100         WriteByte(0x01a6, 0x1b, addr); 
00101         WriteByte(0x01ac, 0x3e, addr); 
00102         WriteByte(0x01a7, 0x1f, addr); 
00103         WriteByte(0x0030, 0x00, addr);
00104         
00105         WriteByte(0x016, 0x00, addr); //change fresh out of set status to 0    
00106     }  
00107     else {
00108         return -1;   
00109     }  
00110     return 0; 
00111 }
00112 
00113 /////////////////////////////////////////////////////////////////// 
00114 // Start a range measurement in single shot mode 
00115 /////////////////////////////////////////////////////////////////// 
00116 int VL6180_Start_Range(char addr) {    
00117     WriteByte(0x018,0x03, addr);  
00118     return 0;
00119 }
00120 
00121 /////////////////////////////////////////////////////////////////// 
00122 // poll for new sample ready ready 
00123 /////////////////////////////////////////////////////////////////// 
00124 int VL6180_Poll_Range(char addr) {    
00125     char status;    
00126     char range_status;  
00127           
00128     // check the status    
00129     status = ReadByte(0x04f,addr);            
00130     range_status = status & 0x07; 
00131            
00132     // wait for new measurement ready status    
00133     while (range_status !=  0x00) 
00134     {         
00135         status = ReadByte(0x04f,addr);        
00136         range_status = status & 0x07;        
00137         //wait_ms(50); // (can be removed)        
00138     }
00139     
00140     return 0;     
00141 }
00142 
00143 
00144 /////////////////////////////////////////////////////////////////// 
00145 // Read range result (mm) 
00146 /////////////////////////////////////////////////////////////////// 
00147 int VL6180_Read_Range(char addr) {    
00148     int range;    
00149     range=ReadByte(0x062, addr);    
00150     return range; 
00151 } 
00152 
00153 /////////////////////////////////////////////////////////////////// 
00154 // clear interrupts 
00155 /////////////////////////////////////////////////////////////////// 
00156 int VL6180_Clear_Interrupts(char addr) {    
00157     WriteByte(0x015,0x07, addr);    
00158     return 0; 
00159 }
00160 
00161 /////////////////////////////////////////////////////////////////// 
00162 //  Main Program loop 
00163 /////////////////////////////////////////////////////////////////// 
00164 int main() {  
00165     SHDN_1 = 0;
00166     SHDN_3 = 0;
00167     SHDN_2 = 0;
00168     SHDN_4 = 0;
00169     wait_ms(0.5);
00170  
00171     SHDN_1 = 0;
00172     SHDN_2 = 1;
00173     SHDN_3 = 0;
00174     SHDN_4 = 0;
00175 //    ros::NodeHandle  nh;
00176 //    nh.initNode();
00177 //    
00178 //    std_msgs::Int32 int_sensor1_msg;
00179 //    std_msgs::Int32 int_sensor2_msg;
00180 //    ros::Publisher range1_pub("sensor1", &int_sensor1_msg);
00181 
00182 //
00183 //    nh.advertise(range1_pub);
00184 //    nh.advertise(range2_pub);
00185     
00186     int range1; 
00187     int range2;
00188     int range3;
00189     int range4;  
00190     
00191     // load settings onto VL6180X sensors    
00192     VL6180_Init(ADDR1);
00193     // change default address of sensor 2
00194     WriteByte(0x212, addr2, ADDR1);
00195     
00196     SHDN_3 = 1;
00197     VL6180_Init(ADDR1);
00198     // change default address of sensor 3
00199     WriteByte(0x212, addr3, ADDR1);
00200     
00201     SHDN_4 = 1;
00202     VL6180_Init(ADDR1);
00203     // change default address of sensor 4
00204     WriteByte(0x212, addr4, ADDR1);
00205     
00206     SHDN_1 = 1;
00207     VL6180_Init(ADDR1);    
00208   
00209     while (1)
00210     {            
00211         // start range measurement          
00212         VL6180_Start_Range(ADDR1);
00213         // poll the VL6180 till new sample ready      
00214         VL6180_Poll_Range(ADDR1);   
00215         range1 = VL6180_Read_Range(ADDR1);
00216         VL6180_Clear_Interrupts(ADDR1);  
00217         wait_ms(10);
00218         // read range result 
00219         VL6180_Start_Range(ADDR2);         
00220         VL6180_Poll_Range(ADDR2); 
00221         range2 = VL6180_Read_Range(ADDR2); 
00222         VL6180_Clear_Interrupts(ADDR2);    
00223         wait_ms(10);
00224         VL6180_Start_Range(ADDR3);         
00225         VL6180_Poll_Range(ADDR3); 
00226         range3 = VL6180_Read_Range(ADDR3);
00227         VL6180_Clear_Interrupts(ADDR3);  
00228         wait_ms(10);
00229         VL6180_Start_Range(ADDR4);         
00230         VL6180_Poll_Range(ADDR4); 
00231         range4 = VL6180_Read_Range(ADDR4);
00232         VL6180_Clear_Interrupts(ADDR4);                 
00233         // clear the interrupt on VL6180                 
00234         // send range to pc by serial
00235          
00236 //        int_sensor1_msg.data = range1;
00237 //        int_sensor2_msg.data = range2;    
00238 //        range1_pub.publish(&int_sensor1_msg);     
00239 //        range2_pub.publish(&int_sensor2_msg);  
00240 //        nh.spinOnce();
00241 
00242         pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);       
00243         wait_ms(100); 
00244         
00245     } 
00246 } 
00247