TOF.cpp

00001 /*--------------------------------------------------------------------------------
00002 Filename: TOF.cpp
00003 Description: Source file for interfacing with the Adafruit TOF sensor
00004 --------------------------------------------------------------------------------*/
00005 
00006 #include "mbed.h"
00007 #include "TOF.h"
00008 
00009 I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM board
00010 
00011 /*--------------------------------------------------------------------------------
00012 Function name: ServiceTOF
00013 Input Parameters: address - address of target TOF sensor
00014 Output Parameters: range - distance measurement in mm
00015 Description: performs measurement routine on a given sensor
00016 ----------------------------------------------------------------------------------*/
00017 uint8_t serviceTOF(cAdafruit_VL6180X VL6180X, uint8_t address){
00018         
00019         uint8_t range = 0;
00020         
00021         // poll the VL6180X till new sample ready
00022         VL6180X.Poll_Range(address);
00023 
00024         // read range result
00025         range = VL6180X.Read_Range(address);
00026         
00027         // clear the interrupt on VL6180X
00028         VL6180X.Clear_Interrupts(address);
00029         
00030         return range;
00031 }
00032 
00033 /*--------------------------------------------------------------------------------
00034 Function name: cAdafruit_VL6180X
00035 Input Parameters: N/A
00036 Output Parameters: N/A
00037 Description: Class constructor (Initialisation upon creating class)
00038 ----------------------------------------------------------------------------------*/
00039 cAdafruit_VL6180X::cAdafruit_VL6180X(DigitalOut sensor1, DigitalOut sensor2, DigitalOut sensor3, DigitalOut sensor4)
00040 {
00041     //Disable all sensors to start
00042     sensor1 = 0;
00043     sensor2 = 0;
00044     sensor3 = 0;
00045     sensor4 = 0;
00046     
00047     //short delay
00048     wait(0.01);
00049 
00050     //Enable and configure sensor 1
00051     sensor1 = 1;
00052     wait(0.01);
00053     Setup(DEFAULT_ADDR, ADDR1); //Change address
00054     Init(ADDR1);        //Perform standard initialisation routine
00055     Start_Range(ADDR1); //Begin sampling in continuous mode
00056     
00057     //Enable and configure sensor 2
00058     sensor2 = 1;
00059     wait(0.01);
00060     Setup(DEFAULT_ADDR, ADDR4); //Change address
00061     Init(ADDR4);        //Perform standard initialisation routine
00062     Start_Range(ADDR4); //Begin sampling in continuous mode
00063     
00064     //Enable and configure sensor 3
00065     sensor3 = 1;
00066     wait(0.01);
00067     Setup(DEFAULT_ADDR, ADDR6); //Change address
00068     Init(ADDR6);        //Perform standard initialisation routine
00069     Start_Range(ADDR6); //Begin sampling in continuous mode
00070     
00071     //Enable and configure sensor 4
00072     sensor4 = 1;
00073     wait(0.01);
00074     Setup(DEFAULT_ADDR, ADDR7); //Change address
00075     Init(ADDR7);        //Perform standard initialisation routine
00076     Start_Range(ADDR7); //Begin sampling in continuous mode
00077     
00078     printf("INITIALISED\n\r");
00079 }
00080 
00081 /*--------------------------------------------------------------------------------
00082 Function name: WriteByte(wchar_t reg,char data)
00083 Input Parameters: reg - the register of the data to write, data - the data to send
00084 Output Parameters: N/A
00085 Description: Writes a single byte to a specified address using the I2C libraries
00086 ----------------------------------------------------------------------------------*/
00087 void cAdafruit_VL6180X::WriteByte(uint8_t address, wchar_t reg,char data)
00088 {
00089     char data_write[3];
00090     data_write[0] = (reg >> 8) & 0xFF;; // MSB of register address
00091     data_write[1] = reg & 0xFF; // LSB of register address
00092     data_write[2] = data & 0xFF;
00093     i2c.write(address, data_write, 3);
00094 }
00095 
00096 /*--------------------------------------------------------------------------------
00097 Function name: ReadByte(wchar_t reg)
00098 Input Parameters: reg - The register of the data to read from
00099 Output Parameters: data_read[0] - The read data
00100 Description: Writes the address to be read from then reads a single byte using the I2C libraries
00101 ----------------------------------------------------------------------------------*/
00102 char cAdafruit_VL6180X::ReadByte(uint8_t address, wchar_t reg)
00103 {
00104     char data_write[2];
00105     char data_read[1];
00106     data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
00107     data_write[1] = reg & 0xFF; // LSB of register address
00108     i2c.write(address, data_write, 2);
00109     i2c.read(address, data_read, 1);
00110     return data_read[0];
00111 }
00112 
00113 /*--------------------------------------------------------------------------------
00114 Function name: Init()
00115 Input Parameters: address - target TOF sensor address
00116 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
00117 Description: Performs initial setup of the TOF sensor, from STM recommended start up config.
00118 ----------------------------------------------------------------------------------*/
00119 int cAdafruit_VL6180X::Init(uint8_t address)
00120 {
00121     char reset;
00122     reset = ReadByte(address, 0x016);
00123     if (reset==1) { // check to see has it be Initialised already
00124     
00125         // Mandatory : private registers
00126         WriteByte(address,0x0207, 0x01);
00127         WriteByte(address,0x0208, 0x01);
00128         WriteByte(address,0x0096, 0x00);
00129         WriteByte(address,0x0097, 0xfd);
00130         WriteByte(address,0x00e3, 0x00);
00131         WriteByte(address,0x00e4, 0x04);
00132         WriteByte(address,0x00e5, 0x02);
00133         WriteByte(address,0x00e6, 0x01);
00134         WriteByte(address,0x00e7, 0x03);
00135         WriteByte(address,0x00f5, 0x02);
00136         WriteByte(address,0x00d9, 0x05);
00137         WriteByte(address,0x00db, 0xce);
00138         WriteByte(address,0x00dc, 0x03);
00139         WriteByte(address,0x00dd, 0xf8);
00140         WriteByte(address,0x009f, 0x00);
00141         WriteByte(address,0x00a3, 0x3c);
00142         WriteByte(address,0x00b7, 0x00);
00143         WriteByte(address,0x00bb, 0x3c);
00144         WriteByte(address,0x00b2, 0x09);
00145         WriteByte(address,0x00ca, 0x09);
00146         WriteByte(address,0x0198, 0x01);
00147         WriteByte(address,0x01b0, 0x17);
00148         WriteByte(address,0x01ad, 0x00);
00149         WriteByte(address,0x00ff, 0x05);
00150         WriteByte(address,0x0100, 0x05);
00151         WriteByte(address,0x0199, 0x05);
00152         WriteByte(address,0x01a6, 0x1b);
00153         WriteByte(address,0x01ac, 0x3e);
00154         WriteByte(address,0x01a7, 0x1f);
00155         WriteByte(address,0x0030, 0x00);
00156         
00157         // Recommended : Public registers - See data sheet for more detail
00158         WriteByte(address,0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes
00159         WriteByte(address,0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time)
00160         WriteByte(address,0x003f, 0x46); // Sets the light and dark gain (upper nibble). Dark gain should not be changed.
00161         WriteByte(address,0x0031, 0xFF); // sets the # of range measurements after which auto calibration of system is performed
00162         WriteByte(address,0x0040, 0x63); // Set ALS integration time to 100ms
00163         WriteByte(address,0x002e, 0x01); // perform a single temperature calibration of the ranging sensor
00164         WriteByte(address,0x001b, 0x09); // Set default ranging inter-measurement period to 100ms
00165         WriteByte(address,0x003e, 0x31); // Set default ALS inter-measurement period to 500ms
00166         WriteByte(address,0x0014, 0x24); // Configures interrupt on ‘New Sample Ready threshold event’
00167         WriteByte(address, 0x016, 0x00); //change fresh out of set status to 0
00168     } 
00169     return 0;
00170 }
00171 
00172 /*--------------------------------------------------------------------------------
00173 Function name: setup()
00174 Input Parameters: address - target TOF sensor address, newaddress - new address for selected TOF sensor
00175 Output Parameters: N/A
00176 Description: Allows configuration of I2C slave addresses for the TOF sensors
00177 ----------------------------------------------------------------------------------*/
00178 void cAdafruit_VL6180X::Setup(uint8_t address, uint8_t newaddress)
00179 {
00180     WriteByte(address, 0x0212, newaddress/2);
00181 }
00182 
00183 /*--------------------------------------------------------------------------------
00184 Function name: Start_range()
00185 Input Parameters: address - target TOF sensor address
00186 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
00187 Description: Allows range measurements to begin in continuous mode
00188 ----------------------------------------------------------------------------------*/
00189 int cAdafruit_VL6180X::Start_Range(uint8_t address)
00190 {
00191     WriteByte(address, 0x018,0x03);
00192     return 0;
00193 }
00194 
00195 /*--------------------------------------------------------------------------------
00196 Function name: Poll_range()
00197 Input Parameters: address - target TOF sensor address
00198 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
00199 Description: poll for new sample ready ready
00200 ----------------------------------------------------------------------------------*/
00201 int cAdafruit_VL6180X::Poll_Range(uint8_t address)
00202 {
00203     char status;
00204     char range_status;
00205 
00206 // check the status
00207     status = ReadByte(address, 0x04f);
00208     range_status = status & 0x07;
00209 
00210 // wait for new measurement ready status
00211     while (range_status != 0x04) {
00212         status = ReadByte(address, 0x04f);
00213         range_status = status & 0x07;
00214         wait_ms(1); // (can be removed)
00215     }
00216     return 0;
00217 }
00218 
00219 /*--------------------------------------------------------------------------------
00220 Function name: Read_range()
00221 Input Parameters: address - target TOF sensor address
00222 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
00223 Description: allows data to be read from a tergetted address (measurement in mm)
00224 ----------------------------------------------------------------------------------*/
00225 int cAdafruit_VL6180X::Read_Range(uint8_t address)
00226 {
00227     int range;
00228     range=ReadByte(address,0x062);
00229     return range;
00230 }
00231 
00232 /*--------------------------------------------------------------------------------
00233 Function name: Read_range()
00234 Input Parameters: address - target TOF sensor address
00235 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
00236 Description: allows interrupt flags to be cleared
00237 ----------------------------------------------------------------------------------*/
00238 int cAdafruit_VL6180X::Clear_Interrupts(uint8_t address)
00239 {
00240     WriteByte(address, 0x015,0x07);
00241     return 0;
00242 }