Soft robot team / Mbed 2 deprecated ros_pub_string_int

publishes hello and 1 time of flight sensor values.

Dependencies:   mbed ros_lib_kinetic

main.cpp@0:b1f7722c376d, 2019-10-16 (annotated)

Committer:
scarter1
Date:
Wed Oct 16 09:22:23 2019 +0000
Revision:
0:b1f7722c376d
publishes hello and 1 time of flight sensor value;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
scarter1 0:b1f7722c376d 1 ///////////////////////////////////////////////////////////////////// Beginning of code///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 2 #include "mbed.h"
scarter1 0:b1f7722c376d 3 #include "ros.h" // Include ros package
scarter1 0:b1f7722c376d 4 #include "std_msgs/String.h" // Include String msg from std_msgs package
scarter1 0:b1f7722c376d 5 #include "std_msgs/Int32.h" // Include Int32 msg from std_msgs package
scarter1 0:b1f7722c376d 6
scarter1 0:b1f7722c376d 7 DigitalOut myled(LED1); // Use led to toggle to make sure code is running
scarter1 0:b1f7722c376d 8 //Serial pc(SERIAL_TX, SERIAL_RX); // set-up serial to pc
scarter1 0:b1f7722c376d 9 I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE
scarter1 0:b1f7722c376d 10 #define addr (0x52) // I²C address of VL6180 shifted by 1 bit
scarter1 0:b1f7722c376d 11 //(0x29 << 1) so the R/W command can be added
scarter1 0:b1f7722c376d 12 ///////////////////////////////////////////////////////////////////// Split 16-bit register address into two bytes and write // the address + data via I²C///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 13 void WriteByte(wchar_t reg,char data){
scarter1 0:b1f7722c376d 14 char data_write[3];
scarter1 0:b1f7722c376d 15 data_write[0]=(reg >> 8)& 0xFF;; // MSB of register address
scarter1 0:b1f7722c376d 16 data_write[1]= reg & 0xFF; // LSB of register address
scarter1 0:b1f7722c376d 17 data_write[2]= data & 0xFF;
scarter1 0:b1f7722c376d 18 i2c.write(addr, data_write, 3);
scarter1 0:b1f7722c376d 19 }
scarter1 0:b1f7722c376d 20
scarter1 0:b1f7722c376d 21 ///////////////////////////////////////////////////////////////////// Split 16-bit register address into two bytes and write // required register address to VL6180 and read the data back///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 22 char ReadByte(wchar_t reg){
scarter1 0:b1f7722c376d 23 char data_write[2];
scarter1 0:b1f7722c376d 24 char data_read[1];
scarter1 0:b1f7722c376d 25 data_write[0]=(reg >> 8)& 0xFF; // MSB of register address
scarter1 0:b1f7722c376d 26 data_write[1]= reg & 0xFF; // LSB of register address
scarter1 0:b1f7722c376d 27 i2c.write(addr, data_write, 2);
scarter1 0:b1f7722c376d 28 i2c.read(addr, data_read, 1);
scarter1 0:b1f7722c376d 29 return data_read[0];
scarter1 0:b1f7722c376d 30 }
scarter1 0:b1f7722c376d 31
scarter1 0:b1f7722c376d 32 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 33 // load settings
scarter1 0:b1f7722c376d 34 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 35 int VL6180_Init(){
scarter1 0:b1f7722c376d 36 char reset;
scarter1 0:b1f7722c376d 37 reset = ReadByte(0x016);
scarter1 0:b1f7722c376d 38 if(reset==1){ // check to see has it be Initialised already
scarter1 0:b1f7722c376d 39 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 40 // Mandatory : private registers
scarter1 0:b1f7722c376d 41 WriteByte(0x0207, 0x01);
scarter1 0:b1f7722c376d 42 WriteByte(0x0208, 0x01);
scarter1 0:b1f7722c376d 43 WriteByte(0x0096, 0x00);
scarter1 0:b1f7722c376d 44 WriteByte(0x0097, 0xfd);
scarter1 0:b1f7722c376d 45 WriteByte(0x00e3, 0x01);
scarter1 0:b1f7722c376d 46 WriteByte(0x00e4, 0x03);
scarter1 0:b1f7722c376d 47 WriteByte(0x00e5, 0x02);
scarter1 0:b1f7722c376d 48 WriteByte(0x00e6, 0x01);
scarter1 0:b1f7722c376d 49 WriteByte(0x00e7, 0x03);
scarter1 0:b1f7722c376d 50 WriteByte(0x00f5, 0x02);
scarter1 0:b1f7722c376d 51 WriteByte(0x00d9, 0x05);
scarter1 0:b1f7722c376d 52 WriteByte(0x00db, 0xce);
scarter1 0:b1f7722c376d 53 WriteByte(0x00dc, 0x03);
scarter1 0:b1f7722c376d 54 WriteByte(0x00dd, 0xf8);
scarter1 0:b1f7722c376d 55 WriteByte(0x009f, 0x00);
scarter1 0:b1f7722c376d 56 WriteByte(0x00a3, 0x3c);
scarter1 0:b1f7722c376d 57 WriteByte(0x00b7, 0x00);
scarter1 0:b1f7722c376d 58 WriteByte(0x00bb, 0x3c);
scarter1 0:b1f7722c376d 59 WriteByte(0x00b2, 0x09);
scarter1 0:b1f7722c376d 60 WriteByte(0x00ca, 0x09);
scarter1 0:b1f7722c376d 61 WriteByte(0x0198, 0x01);
scarter1 0:b1f7722c376d 62 WriteByte(0x01b0, 0x17);
scarter1 0:b1f7722c376d 63 WriteByte(0x01ad, 0x00);
scarter1 0:b1f7722c376d 64 WriteByte(0x00ff, 0x05);
scarter1 0:b1f7722c376d 65 WriteByte(0x0100, 0x05);
scarter1 0:b1f7722c376d 66 WriteByte(0x0199, 0x05);
scarter1 0:b1f7722c376d 67 WriteByte(0x01a6, 0x1b);
scarter1 0:b1f7722c376d 68 WriteByte(0x01ac, 0x3e);
scarter1 0:b1f7722c376d 69 WriteByte(0x01a7, 0x1f);
scarter1 0:b1f7722c376d 70 WriteByte(0x0030, 0x00);// Recommended : Public registers - See data sheet for more detail
scarter1 0:b1f7722c376d 71 WriteByte(0x0011, 0x10); // Enables polling for ‘New Sample ready’
scarter1 0:b1f7722c376d 72 // when measurement completes
scarter1 0:b1f7722c376d 73 WriteByte(0x010a, 0x30); // Set the averaging sample period
scarter1 0:b1f7722c376d 74 // (compromise between lower noise and
scarter1 0:b1f7722c376d 75 // increased execution time)
scarter1 0:b1f7722c376d 76 WriteByte(0x003f, 0x46); // Sets the light and dark gain (upper
scarter1 0:b1f7722c376d 77 // nibble). Dark gain should not be
scarter1 0:b1f7722c376d 78 // changed.
scarter1 0:b1f7722c376d 79 WriteByte(0x0031, 0xFF); // sets the # of range measurements after
scarter1 0:b1f7722c376d 80 // which auto calibration of system is
scarter1 0:b1f7722c376d 81 // performed
scarter1 0:b1f7722c376d 82 WriteByte(0x0041, 0x63); // Set ALS integration time to 100ms
scarter1 0:b1f7722c376d 83 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 84 WriteByte(0x016, 0x00); //change fresh out of set status to 0
scarter1 0:b1f7722c376d 85 }
scarter1 0:b1f7722c376d 86 return 0;
scarter1 0:b1f7722c376d 87 }
scarter1 0:b1f7722c376d 88 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 89 // Start a range measurement in single shot mode
scarter1 0:b1f7722c376d 90 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 91 int VL6180_Start_Range(){
scarter1 0:b1f7722c376d 92 WriteByte(0x018,0x01);
scarter1 0:b1f7722c376d 93 return 0;
scarter1 0:b1f7722c376d 94 }
scarter1 0:b1f7722c376d 95
scarter1 0:b1f7722c376d 96 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 97 // poll for new sample ready ready
scarter1 0:b1f7722c376d 98 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 99 int VL6180_Poll_Range(){
scarter1 0:b1f7722c376d 100 char status;
scarter1 0:b1f7722c376d 101 char range_status;
scarter1 0:b1f7722c376d 102 // check the status
scarter1 0:b1f7722c376d 103 status = ReadByte(0x04f);
scarter1 0:b1f7722c376d 104 range_status = status & 0x07;
scarter1 0:b1f7722c376d 105
scarter1 0:b1f7722c376d 106 // wait for new measurement ready status
scarter1 0:b1f7722c376d 107 while(range_status != 0x00){
scarter1 0:b1f7722c376d 108 status = ReadByte(0x04f);
scarter1 0:b1f7722c376d 109 range_status = status & 0x07;
scarter1 0:b1f7722c376d 110 wait_ms(1); // (can be removed)
scarter1 0:b1f7722c376d 111 }
scarter1 0:b1f7722c376d 112 return 0;
scarter1 0:b1f7722c376d 113 }
scarter1 0:b1f7722c376d 114
scarter1 0:b1f7722c376d 115 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 116 // Read range result (mm)
scarter1 0:b1f7722c376d 117 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 118 int VL6180_Read_Range(){
scarter1 0:b1f7722c376d 119 int range;
scarter1 0:b1f7722c376d 120 range=ReadByte(0x062);
scarter1 0:b1f7722c376d 121 return range;
scarter1 0:b1f7722c376d 122 }
scarter1 0:b1f7722c376d 123 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 124 // clear interrupts
scarter1 0:b1f7722c376d 125 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 126 int VL6180_Clear_Interrupts(){
scarter1 0:b1f7722c376d 127 WriteByte(0x015,0x07);
scarter1 0:b1f7722c376d 128 return 0;
scarter1 0:b1f7722c376d 129 }
scarter1 0:b1f7722c376d 130 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 131 // Main Program loop
scarter1 0:b1f7722c376d 132 ///////////////////////////////////////////////////////////////////
scarter1 0:b1f7722c376d 133 int main()
scarter1 0:b1f7722c376d 134 {
scarter1 0:b1f7722c376d 135 //Initialise node handler
scarter1 0:b1f7722c376d 136 ros::NodeHandle nh;
scarter1 0:b1f7722c376d 137 nh.initNode(); //Initialise rosnode
scarter1 0:b1f7722c376d 138
scarter1 0:b1f7722c376d 139 //Initialises chatter rostopic publisher
scarter1 0:b1f7722c376d 140 std_msgs::String str_msg; // Declares string msg entity
scarter1 0:b1f7722c376d 141 ros::Publisher chatter("chatter", &str_msg); // Declares publisher for rostopic
scarter1 0:b1f7722c376d 142 nh.advertise(chatter); // advertise publisher on ros server
scarter1 0:b1f7722c376d 143
scarter1 0:b1f7722c376d 144 char hello[13] = "hello world!";
scarter1 0:b1f7722c376d 145
scarter1 0:b1f7722c376d 146 //Initialises range rostopic publisher
scarter1 0:b1f7722c376d 147 std_msgs::Int32 int_msg; // Declares integer 32 msg entity
scarter1 0:b1f7722c376d 148 ros::Publisher range_pub("range", &int_msg); // Declares publisher for rostopic
scarter1 0:b1f7722c376d 149 nh.advertise(range_pub); // advertises publisher on ros server
scarter1 0:b1f7722c376d 150
scarter1 0:b1f7722c376d 151 int32_t range;
scarter1 0:b1f7722c376d 152 myled = 0;
scarter1 0:b1f7722c376d 153 //pc.baud(57600);
scarter1 0:b1f7722c376d 154 // load settings onto VL6180X
scarter1 0:b1f7722c376d 155 VL6180_Init();
scarter1 0:b1f7722c376d 156
scarter1 0:b1f7722c376d 157 while (1) {
scarter1 0:b1f7722c376d 158 myled = !myled;
scarter1 0:b1f7722c376d 159 // start single range measurement
scarter1 0:b1f7722c376d 160 VL6180_Start_Range();
scarter1 0:b1f7722c376d 161
scarter1 0:b1f7722c376d 162 // poll the VL6180 till new sample ready
scarter1 0:b1f7722c376d 163 VL6180_Poll_Range();
scarter1 0:b1f7722c376d 164
scarter1 0:b1f7722c376d 165 // read range result
scarter1 0:b1f7722c376d 166 range = VL6180_Read_Range();
scarter1 0:b1f7722c376d 167
scarter1 0:b1f7722c376d 168 //clear the interrupt on VL6180
scarter1 0:b1f7722c376d 169 VL6180_Clear_Interrupts();
scarter1 0:b1f7722c376d 170
scarter1 0:b1f7722c376d 171 //send range to pc by serial
scarter1 0:b1f7722c376d 172 //pc.printf("%d \r\n", range);
scarter1 0:b1f7722c376d 173
scarter1 0:b1f7722c376d 174 str_msg.data = hello; // add hello to data member of string msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/String.html
scarter1 0:b1f7722c376d 175 int_msg.data = range; // add range to data member of integer msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/Int32.html
scarter1 0:b1f7722c376d 176
scarter1 0:b1f7722c376d 177 chatter.publish(&str_msg); // publish chatter rostopic
scarter1 0:b1f7722c376d 178 range_pub.publish(&int_msg); // publish range rostopic
scarter1 0:b1f7722c376d 179
scarter1 0:b1f7722c376d 180 nh.spinOnce(); // checks to see if ros master is running okay
scarter1 0:b1f7722c376d 181 // If ros master isn't working then the code will exit the while loop
scarter1 0:b1f7722c376d 182 wait(0.5);
scarter1 0:b1f7722c376d 183 }
scarter1 0:b1f7722c376d 184
scarter1 0:b1f7722c376d 185 }
scarter1 0:b1f7722c376d 186