Diff: main.cpp

Revision:

0:b1f7722c376d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 16 09:22:23 2019 +0000
@@ -0,0 +1,186 @@
+///////////////////////////////////////////////////////////////////// Beginning of code///////////////////////////////////////////////////////////////////
+#include "mbed.h"
+#include "ros.h" // Include ros package
+#include "std_msgs/String.h" // Include String msg from std_msgs package
+#include "std_msgs/Int32.h" // Include Int32 msg from std_msgs package
+
+DigitalOut myled(LED1); // Use led to toggle to make sure code is running
+//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
+        ///////////////////////////////////////////////////////////////////
+        // 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(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,0x01);
+    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;
+    
+    // wait for new measurement ready status   
+    while(range_status !=  0x00){
+        status = ReadByte(0x04f);
+        range_status = status & 0x07;
+        wait_ms(1); // (can be removed)
+    }
+    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()
+{
+    //Initialise node handler
+    ros::NodeHandle nh;
+    nh.initNode(); //Initialise rosnode
+    
+    //Initialises chatter rostopic publisher
+    std_msgs::String str_msg; // Declares string msg entity
+    ros::Publisher chatter("chatter", &str_msg); // Declares publisher for rostopic
+    nh.advertise(chatter); // advertise publisher on ros server
+    
+    char hello[13] = "hello world!";
+    
+    //Initialises range rostopic publisher
+    std_msgs::Int32 int_msg; // Declares integer 32 msg entity
+    ros::Publisher range_pub("range", &int_msg); // Declares publisher for rostopic
+    nh.advertise(range_pub); // advertises publisher on ros server
+    
+    int32_t range;
+    myled = 0;
+    //pc.baud(57600);
+    // load settings onto VL6180X
+    VL6180_Init();
+    
+    while (1) {
+        myled = !myled;
+        // start single range measurement
+        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
+        //pc.printf("%d \r\n", range);
+        
+        str_msg.data = hello; // add hello to data member of string msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/String.html
+        int_msg.data = range; // add range to data member of integer msg http://docs.ros.org/kinetic/api/std_msgs/html/msg/Int32.html
+        
+        chatter.publish(&str_msg); // publish chatter rostopic
+        range_pub.publish(&int_msg); // publish range rostopic
+        
+        nh.spinOnce(); // checks to see if ros master is running okay 
+                        // If ros master isn't working then the code will exit the while loop
+        wait(0.5);
+    }
+    
+}
+    
\ No newline at end of file