Sensors and motors work / Mbed 2 deprecated nucleo_ros_good

It is supposed to work

Dependencies:   mbed ros_lib_kinetic

main.cpp@8:a936ec892d87, 2019-10-17 (annotated)

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