Light sensor for ROS on Nucleo board
Dependencies: mbed ros_lib_kinetic
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
Generated on Wed Jul 20 2022 17:32:44 by
1.7.2