Sensors and motors work / Mbed 2 deprecated nucleo_ros_good

It is supposed to work

Dependencies:   mbed ros_lib_kinetic

main.cpp@9:2b469b23c42d, 2019-10-17 (annotated)

Committer:
florine_van
Date:
Thu Oct 17 12:22:14 2019 +0000
Revision:
9:2b469b23c42d
Parent:
8:a936ec892d87 
clean code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
florine_van 0:231de8959869 1
florine_van 0:231de8959869 2 #include"mbed.h"
florine_van 0:231de8959869 3 #include <ros.h>
florine_van 0:231de8959869 4 #include <std_msgs/String.h>
florine_van 0:231de8959869 5 #include <std_msgs/Int32.h>
florine_van 0:231de8959869 6
dnulty 4:5170ec66aabb 7 Serial pc(SERIAL_TX, SERIAL_RX); // set-up serial to pc
florine_van 0:231de8959869 8 DigitalOut led = LED1;
dnulty 4:5170ec66aabb 9 DigitalOut SHDN_1(PC_9);
dnulty 4:5170ec66aabb 10 DigitalOut SHDN_2(PC_11);
dnulty 4:5170ec66aabb 11 DigitalOut SHDN_3(PD_2);
dnulty 6:8832bc658845 12 DigitalOut SHDN_4(PG_3);
dnulty 4:5170ec66aabb 13
florine_van 0:231de8959869 14 // set-up serial to pc
florine_van 0:231de8959869 15 I2C i2c(I2C_SDA, I2C_SCL);
florine_van 0:231de8959869 16 // Set up I²C on the STM32 NUCLEO-401RE
florine_van 9:2b469b23c42d 17 #define addr1 (0x29)
florine_van 9:2b469b23c42d 18 #define addr2 (0x2A)
dnulty 6:8832bc658845 19 #define addr3 (0x2B)
dnulty 7:c5017af4c555 20 #define addr4 (0x2C)
dnulty 6:8832bc658845 21
dnulty 6:8832bc658845 22 //Macro for addresses
dnulty 7:c5017af4c555 23 #define ADDR1 (addr1<<1)
dnulty 7:c5017af4c555 24 #define ADDR2 (addr2<<1)
dnulty 7:c5017af4c555 25 #define ADDR3 (addr3<<1)
dnulty 7:c5017af4c555 26 #define ADDR4 (addr4<<1)
florine_van 0:231de8959869 27 // I²C address of VL6180 shifted by 1 bit
florine_van 0:231de8959869 28 //(0x29 << 1) so the R/W command can be added
florine_van 0:231de8959869 29
florine_van 0:231de8959869 30 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 31 // Split 16-bit register address into two bytes and write
florine_van 0:231de8959869 32 // the address + data via I²C
florine_van 0:231de8959869 33 ///////////////////////////////////////////////////////////////////
florine_van 3:3a65e8aae6b6 34 void WriteByte(wchar_t reg,char data, char addr) {
florine_van 0:231de8959869 35 char data_write[3];
florine_van 0:231de8959869 36 data_write[0] = (reg >> 8) & 0xFF;;
florine_van 0:231de8959869 37 // MSB of register address
florine_van 0:231de8959869 38 data_write[1] = reg & 0xFF;
florine_van 0:231de8959869 39 // LSB of register address
florine_van 0:231de8959869 40 data_write[2] = data & 0xFF;
florine_van 0:231de8959869 41 i2c.write(addr, data_write, 3);
florine_van 0:231de8959869 42 }
florine_van 0:231de8959869 43
florine_van 0:231de8959869 44 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 45 // Split 16-bit register address into two bytes and write
florine_van 0:231de8959869 46 // required register address to VL6180 and read the data back
florine_van 0:231de8959869 47 ///////////////////////////////////////////////////////////////////
florine_van 3:3a65e8aae6b6 48 char ReadByte(wchar_t reg, char addr) {
florine_van 0:231de8959869 49 char data_write[2];
florine_van 0:231de8959869 50 char data_read[1];
florine_van 0:231de8959869 51
florine_van 0:231de8959869 52 data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
florine_van 0:231de8959869 53 data_write[1] = reg & 0xFF; // LSB of register address
florine_van 0:231de8959869 54
florine_van 0:231de8959869 55 i2c.write(addr, data_write, 2);
florine_van 0:231de8959869 56 i2c.read(addr, data_read, 1);
florine_van 0:231de8959869 57 return data_read[0];
florine_van 0:231de8959869 58 }
florine_van 0:231de8959869 59
florine_van 0:231de8959869 60 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 61 // load settings
florine_van 0:231de8959869 62 ///////////////////////////////////////////////////////////////////
florine_van 3:3a65e8aae6b6 63 int VL6180_Init(char addr) {
dnulty 4:5170ec66aabb 64 char reset;
florine_van 3:3a65e8aae6b6 65
florine_van 9:2b469b23c42d 66 // check to see has it be Initialised already
florine_van 3:3a65e8aae6b6 67 reset = ReadByte(0x016, addr);
florine_van 0:231de8959869 68 if (reset==1)
florine_van 9:2b469b23c42d 69 {
florine_van 0:231de8959869 70 ///////////////////////////////////////////////////////////////////
florine_van 9:2b469b23c42d 71 // DEFAULT SETTINGS
florine_van 0:231de8959869 72 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 73 // Mandatory : private registers
florine_van 3:3a65e8aae6b6 74 WriteByte(0x0207, 0x01, addr);
florine_van 3:3a65e8aae6b6 75 WriteByte(0x0208, 0x01, addr);
florine_van 3:3a65e8aae6b6 76 WriteByte(0x0096, 0x00, addr);
florine_van 3:3a65e8aae6b6 77 WriteByte(0x0097, 0xfd, addr);
florine_van 3:3a65e8aae6b6 78 WriteByte(0x00e3, 0x01, addr);
florine_van 3:3a65e8aae6b6 79 WriteByte(0x00e4, 0x03, addr);
florine_van 3:3a65e8aae6b6 80 WriteByte(0x00e5, 0x02, addr);
florine_van 3:3a65e8aae6b6 81 WriteByte(0x00e6, 0x01, addr);
florine_van 3:3a65e8aae6b6 82 WriteByte(0x00e7, 0x03, addr);
florine_van 3:3a65e8aae6b6 83 WriteByte(0x00f5, 0x02, addr);
florine_van 3:3a65e8aae6b6 84 WriteByte(0x00d9, 0x05, addr);
florine_van 3:3a65e8aae6b6 85 WriteByte(0x00db, 0xce, addr);
florine_van 3:3a65e8aae6b6 86 WriteByte(0x00dc, 0x03, addr);
florine_van 3:3a65e8aae6b6 87 WriteByte(0x00dd, 0xf8, addr);
florine_van 3:3a65e8aae6b6 88 WriteByte(0x009f, 0x00, addr);
florine_van 3:3a65e8aae6b6 89 WriteByte(0x00a3, 0x3c, addr);
florine_van 3:3a65e8aae6b6 90 WriteByte(0x00b7, 0x00, addr);
florine_van 3:3a65e8aae6b6 91 WriteByte(0x00bb, 0x3c, addr);
florine_van 3:3a65e8aae6b6 92 WriteByte(0x00b2, 0x09, addr);
florine_van 3:3a65e8aae6b6 93 WriteByte(0x00ca, 0x09, addr);
florine_van 3:3a65e8aae6b6 94 WriteByte(0x0198, 0x01, addr);
florine_van 3:3a65e8aae6b6 95 WriteByte(0x01b0, 0x17, addr);
florine_van 3:3a65e8aae6b6 96 WriteByte(0x01ad, 0x00, addr);
florine_van 3:3a65e8aae6b6 97 WriteByte(0x00ff, 0x05, addr);
florine_van 3:3a65e8aae6b6 98 WriteByte(0x0100, 0x05, addr);
florine_van 3:3a65e8aae6b6 99 WriteByte(0x0199, 0x05, addr);
florine_van 3:3a65e8aae6b6 100 WriteByte(0x01a6, 0x1b, addr);
florine_van 3:3a65e8aae6b6 101 WriteByte(0x01ac, 0x3e, addr);
florine_van 3:3a65e8aae6b6 102 WriteByte(0x01a7, 0x1f, addr);
florine_van 3:3a65e8aae6b6 103 WriteByte(0x0030, 0x00, addr);
florine_van 0:231de8959869 104
florine_van 3:3a65e8aae6b6 105 WriteByte(0x016, 0x00, addr); //change fresh out of set status to 0
florine_van 9:2b469b23c42d 106 }
florine_van 9:2b469b23c42d 107 else {
florine_van 9:2b469b23c42d 108 return -1;
florine_van 9:2b469b23c42d 109 }
florine_van 0:231de8959869 110 return 0;
florine_van 0:231de8959869 111 }
florine_van 0:231de8959869 112
florine_van 0:231de8959869 113 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 114 // Start a range measurement in single shot mode
florine_van 0:231de8959869 115 ///////////////////////////////////////////////////////////////////
florine_van 3:3a65e8aae6b6 116 int VL6180_Start_Range(char addr) {
florine_van 3:3a65e8aae6b6 117 WriteByte(0x018,0x03, addr);
florine_van 0:231de8959869 118 return 0;
florine_van 0:231de8959869 119 }
florine_van 0:231de8959869 120
florine_van 0:231de8959869 121 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 122 // poll for new sample ready ready
florine_van 0:231de8959869 123 ///////////////////////////////////////////////////////////////////
florine_van 3:3a65e8aae6b6 124 int VL6180_Poll_Range(char addr) {
florine_van 0:231de8959869 125 char status;
florine_van 0:231de8959869 126 char range_status;
florine_van 0:231de8959869 127
florine_van 0:231de8959869 128 // check the status
florine_van 3:3a65e8aae6b6 129 status = ReadByte(0x04f,addr);
florine_van 0:231de8959869 130 range_status = status & 0x07;
florine_van 0:231de8959869 131
florine_van 0:231de8959869 132 // wait for new measurement ready status
florine_van 3:3a65e8aae6b6 133 while (range_status != 0x00)
florine_van 0:231de8959869 134 {
florine_van 3:3a65e8aae6b6 135 status = ReadByte(0x04f,addr);
florine_van 0:231de8959869 136 range_status = status & 0x07;
dnulty 8:a936ec892d87 137 //wait_ms(50); // (can be removed)
florine_van 0:231de8959869 138 }
florine_van 0:231de8959869 139
florine_van 0:231de8959869 140 return 0;
florine_van 0:231de8959869 141 }
florine_van 0:231de8959869 142
florine_van 3:3a65e8aae6b6 143
florine_van 0:231de8959869 144 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 145 // Read range result (mm)
florine_van 0:231de8959869 146 ///////////////////////////////////////////////////////////////////
florine_van 3:3a65e8aae6b6 147 int VL6180_Read_Range(char addr) {
florine_van 0:231de8959869 148 int range;
florine_van 3:3a65e8aae6b6 149 range=ReadByte(0x062, addr);
florine_van 0:231de8959869 150 return range;
florine_van 0:231de8959869 151 }
florine_van 0:231de8959869 152
florine_van 0:231de8959869 153 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 154 // clear interrupts
florine_van 0:231de8959869 155 ///////////////////////////////////////////////////////////////////
dnulty 7:c5017af4c555 156 int VL6180_Clear_Interrupts(char addr) {
dnulty 7:c5017af4c555 157 WriteByte(0x015,0x07, addr);
florine_van 0:231de8959869 158 return 0;
florine_van 0:231de8959869 159 }
florine_van 0:231de8959869 160
florine_van 0:231de8959869 161 ///////////////////////////////////////////////////////////////////
florine_van 0:231de8959869 162 // Main Program loop
florine_van 0:231de8959869 163 ///////////////////////////////////////////////////////////////////
dnulty 6:8832bc658845 164 int main() {
dnulty 6:8832bc658845 165 SHDN_1 = 0;
dnulty 6:8832bc658845 166 SHDN_3 = 0;
dnulty 6:8832bc658845 167 SHDN_2 = 0;
dnulty 6:8832bc658845 168 SHDN_4 = 0;
dnulty 6:8832bc658845 169 wait_ms(0.5);
florine_van 1:b725652b9b42 170
dnulty 7:c5017af4c555 171 SHDN_1 = 0;
dnulty 7:c5017af4c555 172 SHDN_2 = 1;
dnulty 7:c5017af4c555 173 SHDN_3 = 0;
dnulty 6:8832bc658845 174 SHDN_4 = 0;
dnulty 4:5170ec66aabb 175 // ros::NodeHandle nh;
dnulty 4:5170ec66aabb 176 // nh.initNode();
dnulty 4:5170ec66aabb 177 //
dnulty 4:5170ec66aabb 178 // std_msgs::Int32 int_sensor1_msg;
dnulty 4:5170ec66aabb 179 // std_msgs::Int32 int_sensor2_msg;
dnulty 4:5170ec66aabb 180 // ros::Publisher range1_pub("sensor1", &int_sensor1_msg);
florine_van 1:b725652b9b42 181
dnulty 4:5170ec66aabb 182 //
dnulty 4:5170ec66aabb 183 // nh.advertise(range1_pub);
dnulty 4:5170ec66aabb 184 // nh.advertise(range2_pub);
florine_van 3:3a65e8aae6b6 185
florine_van 3:3a65e8aae6b6 186 int range1;
dnulty 6:8832bc658845 187 int range2;
dnulty 6:8832bc658845 188 int range3;
florine_van 9:2b469b23c42d 189 int range4;
florine_van 1:b725652b9b42 190
florine_van 9:2b469b23c42d 191 // load settings onto VL6180X sensors
dnulty 7:c5017af4c555 192 VL6180_Init(ADDR1);
florine_van 9:2b469b23c42d 193 // change default address of sensor 2
dnulty 7:c5017af4c555 194 WriteByte(0x212, addr2, ADDR1);
florine_van 9:2b469b23c42d 195
dnulty 6:8832bc658845 196 SHDN_3 = 1;
dnulty 7:c5017af4c555 197 VL6180_Init(ADDR1);
florine_van 9:2b469b23c42d 198 // change default address of sensor 3
dnulty 7:c5017af4c555 199 WriteByte(0x212, addr3, ADDR1);
florine_van 9:2b469b23c42d 200
dnulty 7:c5017af4c555 201 SHDN_4 = 1;
dnulty 7:c5017af4c555 202 VL6180_Init(ADDR1);
florine_van 9:2b469b23c42d 203 // change default address of sensor 4
dnulty 7:c5017af4c555 204 WriteByte(0x212, addr4, ADDR1);
florine_van 9:2b469b23c42d 205
dnulty 7:c5017af4c555 206 SHDN_1 = 1;
florine_van 9:2b469b23c42d 207 VL6180_Init(ADDR1);
florine_van 1:b725652b9b42 208
florine_van 0:231de8959869 209 while (1)
florine_van 2:306c6c49a327 210 {
florine_van 9:2b469b23c42d 211 // start range measurement
florine_van 9:2b469b23c42d 212 VL6180_Start_Range(ADDR1);
florine_van 9:2b469b23c42d 213 // poll the VL6180 till new sample ready
florine_van 9:2b469b23c42d 214 VL6180_Poll_Range(ADDR1);
dnulty 7:c5017af4c555 215 range1 = VL6180_Read_Range(ADDR1);
dnulty 7:c5017af4c555 216 VL6180_Clear_Interrupts(ADDR1);
dnulty 8:a936ec892d87 217 wait_ms(10);
florine_van 3:3a65e8aae6b6 218 // read range result
dnulty 7:c5017af4c555 219 VL6180_Start_Range(ADDR2);
dnulty 7:c5017af4c555 220 VL6180_Poll_Range(ADDR2);
dnulty 7:c5017af4c555 221 range2 = VL6180_Read_Range(ADDR2);
dnulty 7:c5017af4c555 222 VL6180_Clear_Interrupts(ADDR2);
dnulty 8:a936ec892d87 223 wait_ms(10);
dnulty 7:c5017af4c555 224 VL6180_Start_Range(ADDR3);
dnulty 7:c5017af4c555 225 VL6180_Poll_Range(ADDR3);
dnulty 7:c5017af4c555 226 range3 = VL6180_Read_Range(ADDR3);
dnulty 7:c5017af4c555 227 VL6180_Clear_Interrupts(ADDR3);
dnulty 8:a936ec892d87 228 wait_ms(10);
dnulty 7:c5017af4c555 229 VL6180_Start_Range(ADDR4);
florine_van 9:2b469b23c42d 230 VL6180_Poll_Range(ADDR4);
dnulty 7:c5017af4c555 231 range4 = VL6180_Read_Range(ADDR4);
dnulty 7:c5017af4c555 232 VL6180_Clear_Interrupts(ADDR4);
dnulty 7:c5017af4c555 233 // clear the interrupt on VL6180
florine_van 1:b725652b9b42 234 // send range to pc by serial
florine_van 1:b725652b9b42 235
dnulty 4:5170ec66aabb 236 // int_sensor1_msg.data = range1;
dnulty 4:5170ec66aabb 237 // int_sensor2_msg.data = range2;
dnulty 4:5170ec66aabb 238 // range1_pub.publish(&int_sensor1_msg);
dnulty 4:5170ec66aabb 239 // range2_pub.publish(&int_sensor2_msg);
dnulty 4:5170ec66aabb 240 // nh.spinOnce();
florine_van 9:2b469b23c42d 241
florine_van 9:2b469b23c42d 242 pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4);
dnulty 7:c5017af4c555 243 wait_ms(100);
florine_van 1:b725652b9b42 244
florine_van 0:231de8959869 245 }
florine_van 0:231de8959869 246 }
florine_van 0:231de8959869 247