Diff: TOFs/TOF.cpp

Revision:

2:6197e1cf2cd1

Parent:

1:9bc7f95c3c7d

Child:

4:36a04230554d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TOFs/TOF.cpp	Thu Oct 24 14:24:27 2019 +0000
@@ -0,0 +1,220 @@
+/*--------------------------------------------------------------------------------
+Filename: TOF.cpp
+Description: Source file for interfacing with the Adafruit TOF sensor
+--------------------------------------------------------------------------------*/
+
+#include "mbed.h"
+#include "TOF.h"
+
+I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM board
+
+/*--------------------------------------------------------------------------------
+Function name: cAdafruit_VL6180X
+Input Parameters: N/A
+Output Parameters: N/A
+Description: Class constructor (Initialisation upon creating class)
+----------------------------------------------------------------------------------*/
+cAdafruit_VL6180X::cAdafruit_VL6180X(DigitalOut sensor1, DigitalOut sensor2, DigitalOut sensor3, DigitalOut sensor4)
+{
+    //Disable all sensors to start
+    sensor1 = 0;
+    sensor2 = 0;
+    sensor3 = 0;
+    sensor4 = 0;
+    
+    //short delay
+    wait(0.01);
+
+    //Enable and configure sensor 1
+    sensor1 = 1;
+    wait(0.01);
+    Setup(DEFAULT_ADDR, ADDR1); //Change address
+    Init(ADDR1);        //Perform standard initialisation routine
+    Start_Range(ADDR1); //Begin sampling in continuous mode
+    
+    //Enable and configure sensor 2
+    sensor2 = 1;
+    wait(0.01);
+    Setup(DEFAULT_ADDR, ADDR2); //Change address
+    Init(ADDR2);        //Perform standard initialisation routine
+    Start_Range(ADDR2); //Begin sampling in continuous mode
+    
+    //Enable and configure sensor 3
+    sensor3 = 1;
+    wait(0.01);
+    Setup(DEFAULT_ADDR, ADDR3); //Change address
+    Init(ADDR3);        //Perform standard initialisation routine
+    Start_Range(ADDR3); //Begin sampling in continuous mode
+    
+    //Enable and configure sensor 4
+    sensor4 = 1;
+    wait(0.01);
+    Setup(DEFAULT_ADDR, ADDR4); //Change address
+    Init(ADDR4);        //Perform standard initialisation routine
+    Start_Range(ADDR4); //Begin sampling in continuous mode
+    
+    printf("INITIALISED\n\r");
+}
+
+/*--------------------------------------------------------------------------------
+Function name: WriteByte(wchar_t reg,char data)
+Input Parameters: reg - the register of the data to write, data - the data to send
+Output Parameters: N/A
+Description: Writes a single byte to a specified address using the I2C libraries
+----------------------------------------------------------------------------------*/
+void cAdafruit_VL6180X::WriteByte(uint8_t address, 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(address, data_write, 3);
+}
+
+/*--------------------------------------------------------------------------------
+Function name: ReadByte(wchar_t reg)
+Input Parameters: reg - The register of the data to read from
+Output Parameters: data_read[0] - The read data
+Description: Writes the address to be read from then reads a single byte using the I2C libraries
+----------------------------------------------------------------------------------*/
+char cAdafruit_VL6180X::ReadByte(uint8_t address, 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(address, data_write, 2);
+    i2c.read(address, data_read, 1);
+    return data_read[0];
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Init()
+Input Parameters: address - target TOF sensor address
+Output Parameters: return 0 as class defined as int (but no meaningful outputs)
+Description: Performs initial setup of the TOF sensor, from STM recommended start up config.
+----------------------------------------------------------------------------------*/
+int cAdafruit_VL6180X::Init(uint8_t address)
+{
+    char reset;
+    reset = ReadByte(address, 0x016);
+    if (reset==1) { // check to see has it be Initialised already
+    
+        // Mandatory : private registers
+        WriteByte(address,0x0207, 0x01);
+        WriteByte(address,0x0208, 0x01);
+        WriteByte(address,0x0096, 0x00);
+        WriteByte(address,0x0097, 0xfd);
+        WriteByte(address,0x00e3, 0x00);
+        WriteByte(address,0x00e4, 0x04);
+        WriteByte(address,0x00e5, 0x02);
+        WriteByte(address,0x00e6, 0x01);
+        WriteByte(address,0x00e7, 0x03);
+        WriteByte(address,0x00f5, 0x02);
+        WriteByte(address,0x00d9, 0x05);
+        WriteByte(address,0x00db, 0xce);
+        WriteByte(address,0x00dc, 0x03);
+        WriteByte(address,0x00dd, 0xf8);
+        WriteByte(address,0x009f, 0x00);
+        WriteByte(address,0x00a3, 0x3c);
+        WriteByte(address,0x00b7, 0x00);
+        WriteByte(address,0x00bb, 0x3c);
+        WriteByte(address,0x00b2, 0x09);
+        WriteByte(address,0x00ca, 0x09);
+        WriteByte(address,0x0198, 0x01);
+        WriteByte(address,0x01b0, 0x17);
+        WriteByte(address,0x01ad, 0x00);
+        WriteByte(address,0x00ff, 0x05);
+        WriteByte(address,0x0100, 0x05);
+        WriteByte(address,0x0199, 0x05);
+        WriteByte(address,0x01a6, 0x1b);
+        WriteByte(address,0x01ac, 0x3e);
+        WriteByte(address,0x01a7, 0x1f);
+        WriteByte(address,0x0030, 0x00);
+        
+        // Recommended : Public registers - See data sheet for more detail
+        WriteByte(address,0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes
+        WriteByte(address,0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time)
+        WriteByte(address,0x003f, 0x46); // Sets the light and dark gain (upper nibble). Dark gain should not be changed.
+        WriteByte(address,0x0031, 0xFF); // sets the # of range measurements after which auto calibration of system is performed
+        WriteByte(address,0x0040, 0x63); // Set ALS integration time to 100ms
+        WriteByte(address,0x002e, 0x01); // perform a single temperature calibration of the ranging sensor
+        WriteByte(address,0x001b, 0x09); // Set default ranging inter-measurement period to 100ms
+        WriteByte(address,0x003e, 0x31); // Set default ALS inter-measurement period to 500ms
+        WriteByte(address,0x0014, 0x24); // Configures interrupt on ‘New Sample Ready threshold event’
+        WriteByte(address, 0x016, 0x00); //change fresh out of set status to 0
+    } 
+    return 0;
+}
+
+/*--------------------------------------------------------------------------------
+Function name: setup()
+Input Parameters: address - target TOF sensor address, newaddress - new address for selected TOF sensor
+Output Parameters: N/A
+Description: Allows configuration of I2C slave addresses for the TOF sensors
+----------------------------------------------------------------------------------*/
+void cAdafruit_VL6180X::Setup(uint8_t address, uint8_t newaddress)
+{
+    WriteByte(address, 0x0212, newaddress/2);
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Start_range()
+Input Parameters: address - target TOF sensor address
+Output Parameters: return 0 as class defined as int (but no meaningful outputs)
+Description: Allows range measurements to begin in continuous mode
+----------------------------------------------------------------------------------*/
+int cAdafruit_VL6180X::Start_Range(uint8_t address)
+{
+    WriteByte(address, 0x018,0x03);
+    return 0;
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Poll_range()
+Input Parameters: address - target TOF sensor address
+Output Parameters: return 0 as class defined as int (but no meaningful outputs)
+Description: poll for new sample ready ready
+----------------------------------------------------------------------------------*/
+int cAdafruit_VL6180X::Poll_Range(uint8_t address)
+{
+    char status;
+    char range_status;
+
+// check the status
+    status = ReadByte(address, 0x04f);
+    range_status = status & 0x07;
+
+// wait for new measurement ready status
+    while (range_status != 0x04) {
+        status = ReadByte(address, 0x04f);
+        range_status = status & 0x07;
+        wait_ms(1); // (can be removed)
+    }
+    return 0;
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Read_range()
+Input Parameters: address - target TOF sensor address
+Output Parameters: return 0 as class defined as int (but no meaningful outputs)
+Description: allows data to be read from a tergetted address (measurement in mm)
+----------------------------------------------------------------------------------*/
+int cAdafruit_VL6180X::Read_Range(uint8_t address)
+{
+    int range;
+    range=ReadByte(address,0x062);
+    return range;
+}
+
+/*--------------------------------------------------------------------------------
+Function name: Read_range()
+Input Parameters: address - target TOF sensor address
+Output Parameters: return 0 as class defined as int (but no meaningful outputs)
+Description: allows interrupt flags to be cleared
+----------------------------------------------------------------------------------*/
+int cAdafruit_VL6180X::Clear_Interrupts(uint8_t address)
+{
+    WriteByte(address, 0x015,0x07);
+    return 0;
+}
\ No newline at end of file