publishes hello and 1 time of flight sensor values.
Dependencies: mbed ros_lib_kinetic
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
Generated on Sat Jul 30 2022 16:01:43 by
1.7.2