publishes hello and 1 time of flight sensor values.

Dependencies:   mbed ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 ///////////////////////////////////////////////////////////////////// Beginning of code///////////////////////////////////////////////////////////////////
00002 #include "mbed.h"
00003 #include "ros.h" // Include ros package
00004 #include "std_msgs/String.h" // Include String msg from std_msgs package
00005 #include "std_msgs/Int32.h" // Include Int32 msg from std_msgs package
00006 
00007 DigitalOut myled(LED1); // Use led to toggle to make sure code is running
00008 //Serial pc(SERIAL_TX, SERIAL_RX); // set-up serial to pc
00009 I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE
00010 #define addr    (0x52) // I²C address of VL6180 shifted by 1 bit
00011                         //(0x29 << 1) so the R/W command can be added
00012 ///////////////////////////////////////////////////////////////////// Split 16-bit register address into two bytes and write // the address + data via I²C///////////////////////////////////////////////////////////////////
00013 void WriteByte(wchar_t reg,char data){     
00014     char data_write[3];    
00015     data_write[0]=(reg >> 8)& 0xFF;; // MSB of register address    
00016     data_write[1]= reg  & 0xFF; // LSB of register address    
00017     data_write[2]= data & 0xFF;    
00018     i2c.write(addr, data_write, 3);
00019 }
00020 
00021 ///////////////////////////////////////////////////////////////////// Split 16-bit register address into two bytes and write // required register address to VL6180 and read the data back///////////////////////////////////////////////////////////////////
00022 char ReadByte(wchar_t reg){    
00023     char data_write[2];    
00024     char data_read[1];    
00025     data_write[0]=(reg >> 8)& 0xFF; // MSB of register address    
00026     data_write[1]= reg  & 0xFF; // LSB of register address    
00027     i2c.write(addr, data_write, 2);    
00028     i2c.read(addr, data_read, 1);
00029     return data_read[0];
00030 }
00031 
00032 ///////////////////////////////////////////////////////////////////
00033 // load settings
00034 ///////////////////////////////////////////////////////////////////
00035 int VL6180_Init(){
00036     char reset;
00037     reset = ReadByte(0x016);
00038     if(reset==1){      // check to see has it be Initialised already
00039         ///////////////////////////////////////////////////////////////////
00040         // Mandatory : private registers
00041         WriteByte(0x0207, 0x01);
00042         WriteByte(0x0208, 0x01);
00043         WriteByte(0x0096, 0x00);
00044         WriteByte(0x0097, 0xfd);
00045         WriteByte(0x00e3, 0x01);
00046         WriteByte(0x00e4, 0x03);
00047         WriteByte(0x00e5, 0x02);
00048         WriteByte(0x00e6, 0x01);
00049         WriteByte(0x00e7, 0x03);
00050         WriteByte(0x00f5, 0x02);
00051         WriteByte(0x00d9, 0x05);
00052         WriteByte(0x00db, 0xce);
00053         WriteByte(0x00dc, 0x03);
00054         WriteByte(0x00dd, 0xf8);
00055         WriteByte(0x009f, 0x00);
00056         WriteByte(0x00a3, 0x3c);
00057         WriteByte(0x00b7, 0x00);
00058         WriteByte(0x00bb, 0x3c);
00059         WriteByte(0x00b2, 0x09);
00060         WriteByte(0x00ca, 0x09);
00061         WriteByte(0x0198, 0x01);
00062         WriteByte(0x01b0, 0x17);
00063         WriteByte(0x01ad, 0x00);
00064         WriteByte(0x00ff, 0x05);
00065         WriteByte(0x0100, 0x05);
00066         WriteByte(0x0199, 0x05);
00067         WriteByte(0x01a6, 0x1b);
00068         WriteByte(0x01ac, 0x3e);
00069         WriteByte(0x01a7, 0x1f);
00070         WriteByte(0x0030, 0x00);// Recommended : Public registers - See data sheet for more detail
00071         WriteByte(0x0011, 0x10); // Enables polling for ‘New Sample ready’  
00072         // when measurement completes
00073         WriteByte(0x010a, 0x30); // Set the averaging sample period 
00074         // (compromise between lower noise and  
00075         // increased execution time)
00076         WriteByte(0x003f, 0x46); // Sets the light and dark gain (upper  
00077         // nibble). Dark gain should not be                         
00078         //    changed.
00079         WriteByte(0x0031, 0xFF); // sets the # of range measurements after  
00080         // which auto calibration of system is 
00081         // performed 
00082         WriteByte(0x0041, 0x63); // Set ALS integration time to 100ms
00083         ///////////////////////////////////////////////////////////////////
00084             WriteByte(0x016, 0x00); //change fresh out of set status to 0
00085     }
00086     return 0;
00087 }
00088 ///////////////////////////////////////////////////////////////////
00089 // Start a range measurement in single shot mode
00090 ///////////////////////////////////////////////////////////////////
00091 int VL6180_Start_Range(){
00092     WriteByte(0x018,0x01);
00093     return 0;
00094 }
00095 
00096 ///////////////////////////////////////////////////////////////////
00097 // poll for new sample ready ready
00098 ///////////////////////////////////////////////////////////////////
00099 int VL6180_Poll_Range(){
00100     char status;
00101     char range_status;
00102     // check the status
00103     status = ReadByte(0x04f);
00104     range_status = status & 0x07;
00105     
00106     // wait for new measurement ready status   
00107     while(range_status !=  0x00){
00108         status = ReadByte(0x04f);
00109         range_status = status & 0x07;
00110         wait_ms(1); // (can be removed)
00111     }
00112     return 0;
00113 }
00114 
00115 ///////////////////////////////////////////////////////////////////    
00116 // Read range result (mm)
00117 ///////////////////////////////////////////////////////////////////
00118 int VL6180_Read_Range(){
00119     int range;
00120     range=ReadByte(0x062);
00121     return range;
00122 }
00123 ///////////////////////////////////////////////////////////////////
00124 // clear interrupts
00125 ///////////////////////////////////////////////////////////////////
00126 int VL6180_Clear_Interrupts(){
00127     WriteByte(0x015,0x07);
00128     return 0;
00129 }
00130 ///////////////////////////////////////////////////////////////////
00131 //  Main Program loop
00132 ///////////////////////////////////////////////////////////////////
00133 int main()
00134 {
00135     //Initialise node handler
00136     ros::NodeHandle nh;
00137     nh.initNode(); //Initialise rosnode
00138     
00139     //Initialises chatter rostopic publisher
00140     std_msgs::String str_msg; // Declares string msg entity
00141     ros::Publisher chatter("chatter", &str_msg); // Declares publisher for rostopic
00142     nh.advertise(chatter); // advertise publisher on ros server
00143     
00144     char hello[13] = "hello world!";
00145     
00146     //Initialises range rostopic publisher
00147     std_msgs::Int32 int_msg; // Declares integer 32 msg entity
00148     ros::Publisher range_pub("range", &int_msg); // Declares publisher for rostopic
00149     nh.advertise(range_pub); // advertises publisher on ros server
00150     
00151     int32_t range;
00152     myled = 0;
00153     //pc.baud(57600);
00154     // load settings onto VL6180X
00155     VL6180_Init();
00156     
00157     while (1) {
00158         myled = !myled;
00159         // start single range measurement
00160         VL6180_Start_Range();
00161         
00162         // poll the VL6180 till new sample ready
00163         VL6180_Poll_Range();
00164         
00165         // read range result
00166         range = VL6180_Read_Range();
00167         
00168         //clear the interrupt on VL6180
00169         VL6180_Clear_Interrupts();
00170         
00171         //send range to pc by serial
00172         //pc.printf("%d \r\n", range);
00173         
00174         str_msg.data = hello; // add hello to data member of string msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/String.html
00175         int_msg.data = range; // add range to data member of integer msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/Int32.html
00176         
00177         chatter.publish(&str_msg); // publish chatter rostopic
00178         range_pub.publish(&int_msg); // publish range rostopic
00179         
00180         nh.spinOnce(); // checks to see if ros master is running okay 
00181                         // If ros master isn't working then the code will exit the while loop
00182         wait(0.5);
00183     }
00184     
00185 }
00186