It is supposed to work
Dependencies: mbed ros_lib_kinetic
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
Generated on Wed Aug 3 2022 23:24:56 by
1.7.2