Sensors and motors work / Mbed 2 deprecated nucleo_ros_good

It is supposed to work

Dependencies:   mbed ros_lib_kinetic

main.cpp@3:3a65e8aae6b6, 2019-10-15 (annotated)

Committer:
florine_van
Date:
Tue Oct 15 12:20:06 2019 +0000
Revision:
3:3a65e8aae6b6
Parent:
2:306c6c49a327
Child:
4:5170ec66aabb
Code for two sensors

Who changed what in which revision?

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