Sensors and motors work / Mbed 2 deprecated nucleo_ros_good

It is supposed to work

Dependencies:   mbed ros_lib_kinetic

main.cpp@6:8832bc658845, 2019-10-17 (annotated)

Committer:
dnulty
Date:
Thu Oct 17 11:37:04 2019 +0000
Revision:
6:8832bc658845
Parent:
5:4a24d8597fc3
Child:
7:c5017af4c555
3 sensors working ;

Who changed what in which revision?

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