Sensors and motors work / Mbed 2 deprecated nucleo_ros_good

It is supposed to work

Dependencies:   mbed ros_lib_kinetic

main.cpp@0:231de8959869, 2019-10-09 (annotated)

Committer:
florine_van
Date:
Wed Oct 09 08:08:38 2019 +0000
Revision:
0:231de8959869
Child:
1:b725652b9b42
1st revision

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