Diff: main.cpp

Revision:

0:231de8959869

Child:

1:b725652b9b42

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 09 08:08:38 2019 +0000
@@ -0,0 +1,209 @@
+/*
+ * rosserial Publisher Example
+ * Prints "hello world!"
+ */
+
+#include"mbed.h"
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int32.h>
+
+ros::NodeHandle  nh;
+std_msgs::Int32 int_msg;
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+ros::Publisher range_pub("range", &int_msg);
+
+char hello[13] = "hello world!";
+
+DigitalOut led = LED1;
+
+Serial pc(SERIAL_TX, SERIAL_RX); 
+// set-up serial to pc 
+I2C i2c(I2C_SDA, I2C_SCL); 
+// Set up I²C on the STM32 NUCLEO-401RE 
+#define addr    (0x52) 
+// I²C address of VL6180 shifted by 1 bit                        
+//(0x29 << 1) so the R/W command can be added
+
+/////////////////////////////////////////////////////////////////// 
+// Split 16-bit register address into two bytes and write 
+// the address + data via I²C 
+/////////////////////////////////////////////////////////////////// 
+void WriteByte(wchar_t reg,char data) {     
+    char data_write[3];
+    data_write[0] = (reg >> 8) & 0xFF;; 
+    // MSB of register address    
+    data_write[1] = reg  & 0xFF; 
+    // LSB of register address    
+    data_write[2] = data & 0xFF;
+    i2c.write(addr, data_write, 3); 
+}
+
+/////////////////////////////////////////////////////////////////// 
+// Split 16-bit register address into two bytes and write 
+// required register address to VL6180 and read the data back 
+/////////////////////////////////////////////////////////////////// 
+char ReadByte(wchar_t reg) {    
+    char data_write[2];    
+    char data_read[1];
+    
+    data_write[0] = (reg >> 8) & 0xFF; // MSB of register address    
+    data_write[1] = reg  & 0xFF; // LSB of register address
+    
+    i2c.write(addr, data_write, 2);    
+    i2c.read(addr, data_read, 1);    
+    return data_read[0]; 
+}
+    
+/////////////////////////////////////////////////////////////////// 
+// load settings 
+/////////////////////////////////////////////////////////////////// 
+int VL6180_Init() {    
+    char reset;
+    reset = ReadByte(0x016);    
+    if (reset==1)
+    {      
+        // check to see has it be Initialised already
+    
+        /////////////////////////////////////////////////////////////////// 
+        // Added latest settings here - see Section 9 
+        ///////////////////////////////////////////////////////////////////
+        // Mandatory : private registers 
+        WriteByte(0x0207, 0x01); 
+        WriteByte(0x0208, 0x01); 
+        WriteByte(0x0096, 0x00); 
+        WriteByte(0x0097, 0xfd); 
+        WriteByte(0x00e3, 0x01); 
+        WriteByte(0x00e4, 0x03); 
+        WriteByte(0x00e5, 0x02); 
+        WriteByte(0x00e6, 0x01); 
+        WriteByte(0x00e7, 0x03); 
+        WriteByte(0x00f5, 0x02); 
+        WriteByte(0x00d9, 0x05); 
+        WriteByte(0x00db, 0xce); 
+        WriteByte(0x00dc, 0x03); 
+        WriteByte(0x00dd, 0xf8); 
+        WriteByte(0x009f, 0x00); 
+        WriteByte(0x00a3, 0x3c); 
+        WriteByte(0x00b7, 0x00); 
+        WriteByte(0x00bb, 0x3c); 
+        WriteByte(0x00b2, 0x09); 
+        WriteByte(0x00ca, 0x09); 
+        WriteByte(0x0198, 0x01); 
+        WriteByte(0x01b0, 0x17); 
+        WriteByte(0x01ad, 0x00); 
+        WriteByte(0x00ff, 0x05); 
+        WriteByte(0x0100, 0x05); 
+        WriteByte(0x0199, 0x05); 
+        WriteByte(0x01a6, 0x1b); 
+        WriteByte(0x01ac, 0x3e); 
+        WriteByte(0x01a7, 0x1f); 
+        WriteByte(0x0030, 0x00);
+        
+        // Recommended : Public registers - See data sheet for more detail 
+        //WriteByte(0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes 
+        //WriteByte(0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time) 
+        //WriteByte(0x003f, 0x46); // Sets the light and dark gain (upper  // nibble). Dark gain should not be       // changed. 
+        //WriteByte(0x0031, 0xFF); // sets the # of range measurements after  // which auto calibration of system is // performed 
+        //WriteByte(0x0041, 0x63); // Set ALS integration time to 100ms
+        //WriteByte(0x002e, 0x01); // perform a single temperature calibration // of the ranging sensor         
+        
+        WriteByte(0x016, 0x00); //change fresh out of set status to 0    
+    }    
+    return 0; 
+}
+
+/////////////////////////////////////////////////////////////////// 
+// Start a range measurement in single shot mode 
+/////////////////////////////////////////////////////////////////// 
+int VL6180_Start_Range() {    
+    WriteByte(0x018,0x03);    
+    return 0;
+}
+
+/////////////////////////////////////////////////////////////////// 
+// poll for new sample ready ready 
+/////////////////////////////////////////////////////////////////// 
+int VL6180_Poll_Range() {    
+    char status;    
+    char range_status;  
+          
+    // check the status    
+    status = ReadByte(0x04f);            
+    range_status = status & 0x07; 
+    
+    printf("status en cours \n");
+           
+    // wait for new measurement ready status    
+    while (range_status !=  0x00) 
+    {         
+        printf("status = %d\n", status);
+        status = ReadByte(0x04f);        
+        range_status = status & 0x07;        
+        wait_ms(1); // (can be removed)        
+    }
+    
+    //printf("range polled \n");
+    return 0;     
+}
+
+/////////////////////////////////////////////////////////////////// 
+// Read range result (mm) 
+/////////////////////////////////////////////////////////////////// 
+int VL6180_Read_Range() {    
+    int range;    
+    range=ReadByte(0x062);    
+    return range; 
+} 
+
+/////////////////////////////////////////////////////////////////// 
+// clear interrupts 
+/////////////////////////////////////////////////////////////////// 
+int VL6180_Clear_Interrupts() {    
+    WriteByte(0x015,0x07);    
+    return 0; 
+}
+
+/////////////////////////////////////////////////////////////////// 
+//  Main Program loop 
+/////////////////////////////////////////////////////////////////// 
+int main() {    
+    
+    nh.initNode();
+    nh.advertise(chatter);
+    nh.advertise(range_pub);
+    int range; 
+    // load settings onto VL6180X    
+    VL6180_Init(); 
+    
+    //char byte;
+    //byte = ReadByte(0x04d);
+    printf("RESULT_RANGE_STATUS %d\n ", byte);
+    
+    //wait(1);
+    
+    // start single range measurement        
+    
+    
+    while (1)
+    {    
+        led = !led;
+        str_msg.data = hello;
+        chatter.publish( &str_msg );
+        VL6180_Start_Range();  
+        // poll the VL6180 till new sample ready        
+        VL6180_Poll_Range();              
+        // read range result        
+        range = VL6180_Read_Range();                         
+        // clear the interrupt on VL6180        
+        VL6180_Clear_Interrupts();                 
+        // send range to pc by serial 
+        int_msg.data = range;    
+        range_pub.publish(&int_msg);  
+        pc.printf("%d\r\n", range);         
+        wait(0.5); 
+        nh.spinOnce();     
+    } 
+} 
+