SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v46666

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo ros_lib_kinetic

TOFs/TOF.cpp@10:276cc357015c, 2020-01-05 (annotated)

Committer:
hongyunAHN
Date:
Sun Jan 05 21:47:14 2020 +0000
Revision:
10:276cc357015c
Parent:
4:36a04230554d 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stumi 1:9bc7f95c3c7d 1 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 2 Filename: TOF.cpp
Stumi 1:9bc7f95c3c7d 3 Description: Source file for interfacing with the Adafruit TOF sensor
Stumi 1:9bc7f95c3c7d 4 --------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 5
Stumi 1:9bc7f95c3c7d 6 #include "mbed.h"
Stumi 1:9bc7f95c3c7d 7 #include "TOF.h"
Stumi 1:9bc7f95c3c7d 8
Stumi 1:9bc7f95c3c7d 9 I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM board
Stumi 1:9bc7f95c3c7d 10
Stumi 1:9bc7f95c3c7d 11 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 12 Function name: cAdafruit_VL6180X
Stumi 1:9bc7f95c3c7d 13 Input Parameters: N/A
Stumi 1:9bc7f95c3c7d 14 Output Parameters: N/A
Stumi 1:9bc7f95c3c7d 15 Description: Class constructor (Initialisation upon creating class)
Stumi 1:9bc7f95c3c7d 16 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 17 cAdafruit_VL6180X::cAdafruit_VL6180X(DigitalOut sensor1, DigitalOut sensor2, DigitalOut sensor3, DigitalOut sensor4)
Stumi 1:9bc7f95c3c7d 18 {
Stumi 1:9bc7f95c3c7d 19 //Disable all sensors to start
Stumi 1:9bc7f95c3c7d 20 sensor1 = 0;
Stumi 1:9bc7f95c3c7d 21 sensor2 = 0;
Stumi 1:9bc7f95c3c7d 22 sensor3 = 0;
Stumi 1:9bc7f95c3c7d 23 sensor4 = 0;
Stumi 1:9bc7f95c3c7d 24
Stumi 1:9bc7f95c3c7d 25 //short delay
Stumi 1:9bc7f95c3c7d 26 wait(0.01);
Stumi 1:9bc7f95c3c7d 27
Stumi 1:9bc7f95c3c7d 28 //Enable and configure sensor 1
Stumi 1:9bc7f95c3c7d 29 sensor1 = 1;
Stumi 1:9bc7f95c3c7d 30 wait(0.01);
Stumi 1:9bc7f95c3c7d 31 Setup(DEFAULT_ADDR, ADDR1); //Change address
Stumi 1:9bc7f95c3c7d 32 Init(ADDR1); //Perform standard initialisation routine
Stumi 1:9bc7f95c3c7d 33 Start_Range(ADDR1); //Begin sampling in continuous mode
Stumi 1:9bc7f95c3c7d 34
Stumi 1:9bc7f95c3c7d 35 //Enable and configure sensor 2
Stumi 1:9bc7f95c3c7d 36 sensor2 = 1;
Stumi 1:9bc7f95c3c7d 37 wait(0.01);
hongyunAHN 10:276cc357015c 38 Setup(DEFAULT_ADDR, ADDR2); //Change address
hongyunAHN 10:276cc357015c 39 Init(ADDR2); //Perform standard initialisation routine
hongyunAHN 10:276cc357015c 40 Start_Range(ADDR2); //Begin sampling in continuous mode
Stumi 1:9bc7f95c3c7d 41
Stumi 1:9bc7f95c3c7d 42 //Enable and configure sensor 3
Stumi 1:9bc7f95c3c7d 43 sensor3 = 1;
Stumi 1:9bc7f95c3c7d 44 wait(0.01);
hongyunAHN 10:276cc357015c 45 Setup(DEFAULT_ADDR, ADDR3); //Change address
hongyunAHN 10:276cc357015c 46 Init(ADDR3); //Perform standard initialisation routine
hongyunAHN 10:276cc357015c 47 Start_Range(ADDR3); //Begin sampling in continuous mode
Stumi 1:9bc7f95c3c7d 48
Stumi 1:9bc7f95c3c7d 49 //Enable and configure sensor 4
Stumi 1:9bc7f95c3c7d 50 sensor4 = 1;
Stumi 1:9bc7f95c3c7d 51 wait(0.01);
hongyunAHN 10:276cc357015c 52 Setup(DEFAULT_ADDR, ADDR4); //Change address
hongyunAHN 10:276cc357015c 53 Init(ADDR4); //Perform standard initialisation routine
hongyunAHN 10:276cc357015c 54 Start_Range(ADDR4); //Begin sampling in continuous mode
Stumi 1:9bc7f95c3c7d 55
Stumi 1:9bc7f95c3c7d 56 printf("INITIALISED\n\r");
Stumi 1:9bc7f95c3c7d 57 }
Stumi 1:9bc7f95c3c7d 58
Stumi 1:9bc7f95c3c7d 59 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 60 Function name: WriteByte(wchar_t reg,char data)
Stumi 1:9bc7f95c3c7d 61 Input Parameters: reg - the register of the data to write, data - the data to send
Stumi 1:9bc7f95c3c7d 62 Output Parameters: N/A
Stumi 1:9bc7f95c3c7d 63 Description: Writes a single byte to a specified address using the I2C libraries
Stumi 1:9bc7f95c3c7d 64 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 65 void cAdafruit_VL6180X::WriteByte(uint8_t address, wchar_t reg,char data)
Stumi 1:9bc7f95c3c7d 66 {
Stumi 1:9bc7f95c3c7d 67 char data_write[3];
Stumi 1:9bc7f95c3c7d 68 data_write[0] = (reg >> 8) & 0xFF;; // MSB of register address
Stumi 1:9bc7f95c3c7d 69 data_write[1] = reg & 0xFF; // LSB of register address
Stumi 1:9bc7f95c3c7d 70 data_write[2] = data & 0xFF;
Stumi 1:9bc7f95c3c7d 71 i2c.write(address, data_write, 3);
Stumi 1:9bc7f95c3c7d 72 }
Stumi 1:9bc7f95c3c7d 73
Stumi 1:9bc7f95c3c7d 74 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 75 Function name: ReadByte(wchar_t reg)
Stumi 1:9bc7f95c3c7d 76 Input Parameters: reg - The register of the data to read from
Stumi 1:9bc7f95c3c7d 77 Output Parameters: data_read[0] - The read data
Stumi 1:9bc7f95c3c7d 78 Description: Writes the address to be read from then reads a single byte using the I2C libraries
Stumi 1:9bc7f95c3c7d 79 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 80 char cAdafruit_VL6180X::ReadByte(uint8_t address, wchar_t reg)
Stumi 1:9bc7f95c3c7d 81 {
Stumi 1:9bc7f95c3c7d 82 char data_write[2];
Stumi 1:9bc7f95c3c7d 83 char data_read[1];
Stumi 1:9bc7f95c3c7d 84 data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
Stumi 1:9bc7f95c3c7d 85 data_write[1] = reg & 0xFF; // LSB of register address
Stumi 1:9bc7f95c3c7d 86 i2c.write(address, data_write, 2);
Stumi 1:9bc7f95c3c7d 87 i2c.read(address, data_read, 1);
Stumi 1:9bc7f95c3c7d 88 return data_read[0];
Stumi 1:9bc7f95c3c7d 89 }
Stumi 1:9bc7f95c3c7d 90
Stumi 1:9bc7f95c3c7d 91 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 92 Function name: Init()
Stumi 1:9bc7f95c3c7d 93 Input Parameters: address - target TOF sensor address
Stumi 1:9bc7f95c3c7d 94 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Stumi 1:9bc7f95c3c7d 95 Description: Performs initial setup of the TOF sensor, from STM recommended start up config.
Stumi 1:9bc7f95c3c7d 96 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 97 int cAdafruit_VL6180X::Init(uint8_t address)
Stumi 1:9bc7f95c3c7d 98 {
Stumi 1:9bc7f95c3c7d 99 char reset;
Stumi 1:9bc7f95c3c7d 100 reset = ReadByte(address, 0x016);
Stumi 1:9bc7f95c3c7d 101 if (reset==1) { // check to see has it be Initialised already
Stumi 1:9bc7f95c3c7d 102
Stumi 1:9bc7f95c3c7d 103 // Mandatory : private registers
Stumi 1:9bc7f95c3c7d 104 WriteByte(address,0x0207, 0x01);
Stumi 1:9bc7f95c3c7d 105 WriteByte(address,0x0208, 0x01);
Stumi 1:9bc7f95c3c7d 106 WriteByte(address,0x0096, 0x00);
Stumi 1:9bc7f95c3c7d 107 WriteByte(address,0x0097, 0xfd);
Stumi 1:9bc7f95c3c7d 108 WriteByte(address,0x00e3, 0x00);
Stumi 1:9bc7f95c3c7d 109 WriteByte(address,0x00e4, 0x04);
Stumi 1:9bc7f95c3c7d 110 WriteByte(address,0x00e5, 0x02);
Stumi 1:9bc7f95c3c7d 111 WriteByte(address,0x00e6, 0x01);
Stumi 1:9bc7f95c3c7d 112 WriteByte(address,0x00e7, 0x03);
Stumi 1:9bc7f95c3c7d 113 WriteByte(address,0x00f5, 0x02);
Stumi 1:9bc7f95c3c7d 114 WriteByte(address,0x00d9, 0x05);
Stumi 1:9bc7f95c3c7d 115 WriteByte(address,0x00db, 0xce);
Stumi 1:9bc7f95c3c7d 116 WriteByte(address,0x00dc, 0x03);
Stumi 1:9bc7f95c3c7d 117 WriteByte(address,0x00dd, 0xf8);
Stumi 1:9bc7f95c3c7d 118 WriteByte(address,0x009f, 0x00);
Stumi 1:9bc7f95c3c7d 119 WriteByte(address,0x00a3, 0x3c);
Stumi 1:9bc7f95c3c7d 120 WriteByte(address,0x00b7, 0x00);
Stumi 1:9bc7f95c3c7d 121 WriteByte(address,0x00bb, 0x3c);
Stumi 1:9bc7f95c3c7d 122 WriteByte(address,0x00b2, 0x09);
Stumi 1:9bc7f95c3c7d 123 WriteByte(address,0x00ca, 0x09);
Stumi 1:9bc7f95c3c7d 124 WriteByte(address,0x0198, 0x01);
Stumi 1:9bc7f95c3c7d 125 WriteByte(address,0x01b0, 0x17);
Stumi 1:9bc7f95c3c7d 126 WriteByte(address,0x01ad, 0x00);
Stumi 1:9bc7f95c3c7d 127 WriteByte(address,0x00ff, 0x05);
Stumi 1:9bc7f95c3c7d 128 WriteByte(address,0x0100, 0x05);
Stumi 1:9bc7f95c3c7d 129 WriteByte(address,0x0199, 0x05);
Stumi 1:9bc7f95c3c7d 130 WriteByte(address,0x01a6, 0x1b);
Stumi 1:9bc7f95c3c7d 131 WriteByte(address,0x01ac, 0x3e);
Stumi 1:9bc7f95c3c7d 132 WriteByte(address,0x01a7, 0x1f);
Stumi 1:9bc7f95c3c7d 133 WriteByte(address,0x0030, 0x00);
Stumi 1:9bc7f95c3c7d 134
Stumi 1:9bc7f95c3c7d 135 // Recommended : Public registers - See data sheet for more detail
Stumi 1:9bc7f95c3c7d 136 WriteByte(address,0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes
Stumi 1:9bc7f95c3c7d 137 WriteByte(address,0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time)
Stumi 1:9bc7f95c3c7d 138 WriteByte(address,0x003f, 0x46); // Sets the light and dark gain (upper nibble). Dark gain should not be changed.
Stumi 1:9bc7f95c3c7d 139 WriteByte(address,0x0031, 0xFF); // sets the # of range measurements after which auto calibration of system is performed
Stumi 1:9bc7f95c3c7d 140 WriteByte(address,0x0040, 0x63); // Set ALS integration time to 100ms
Stumi 1:9bc7f95c3c7d 141 WriteByte(address,0x002e, 0x01); // perform a single temperature calibration of the ranging sensor
Stumi 1:9bc7f95c3c7d 142 WriteByte(address,0x001b, 0x09); // Set default ranging inter-measurement period to 100ms
Stumi 1:9bc7f95c3c7d 143 WriteByte(address,0x003e, 0x31); // Set default ALS inter-measurement period to 500ms
Stumi 1:9bc7f95c3c7d 144 WriteByte(address,0x0014, 0x24); // Configures interrupt on ‘New Sample Ready threshold event’
Stumi 1:9bc7f95c3c7d 145 WriteByte(address, 0x016, 0x00); //change fresh out of set status to 0
Stumi 1:9bc7f95c3c7d 146 }
Stumi 1:9bc7f95c3c7d 147 return 0;
Stumi 1:9bc7f95c3c7d 148 }
Stumi 1:9bc7f95c3c7d 149
Stumi 1:9bc7f95c3c7d 150 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 151 Function name: setup()
Stumi 1:9bc7f95c3c7d 152 Input Parameters: address - target TOF sensor address, newaddress - new address for selected TOF sensor
Stumi 1:9bc7f95c3c7d 153 Output Parameters: N/A
Stumi 1:9bc7f95c3c7d 154 Description: Allows configuration of I2C slave addresses for the TOF sensors
Stumi 1:9bc7f95c3c7d 155 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 156 void cAdafruit_VL6180X::Setup(uint8_t address, uint8_t newaddress)
Stumi 1:9bc7f95c3c7d 157 {
Stumi 1:9bc7f95c3c7d 158 WriteByte(address, 0x0212, newaddress/2);
Stumi 1:9bc7f95c3c7d 159 }
Stumi 1:9bc7f95c3c7d 160
Stumi 1:9bc7f95c3c7d 161 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 162 Function name: Start_range()
Stumi 1:9bc7f95c3c7d 163 Input Parameters: address - target TOF sensor address
Stumi 1:9bc7f95c3c7d 164 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Stumi 1:9bc7f95c3c7d 165 Description: Allows range measurements to begin in continuous mode
Stumi 1:9bc7f95c3c7d 166 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 167 int cAdafruit_VL6180X::Start_Range(uint8_t address)
Stumi 1:9bc7f95c3c7d 168 {
Stumi 1:9bc7f95c3c7d 169 WriteByte(address, 0x018,0x03);
Stumi 1:9bc7f95c3c7d 170 return 0;
Stumi 1:9bc7f95c3c7d 171 }
Stumi 1:9bc7f95c3c7d 172
Stumi 1:9bc7f95c3c7d 173 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 174 Function name: Poll_range()
Stumi 1:9bc7f95c3c7d 175 Input Parameters: address - target TOF sensor address
Stumi 1:9bc7f95c3c7d 176 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Stumi 1:9bc7f95c3c7d 177 Description: poll for new sample ready ready
Stumi 1:9bc7f95c3c7d 178 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 179 int cAdafruit_VL6180X::Poll_Range(uint8_t address)
Stumi 1:9bc7f95c3c7d 180 {
Stumi 1:9bc7f95c3c7d 181 char status;
Stumi 1:9bc7f95c3c7d 182 char range_status;
Stumi 1:9bc7f95c3c7d 183
Stumi 1:9bc7f95c3c7d 184 // check the status
Stumi 1:9bc7f95c3c7d 185 status = ReadByte(address, 0x04f);
Stumi 1:9bc7f95c3c7d 186 range_status = status & 0x07;
Stumi 1:9bc7f95c3c7d 187
Stumi 1:9bc7f95c3c7d 188 // wait for new measurement ready status
Stumi 1:9bc7f95c3c7d 189 while (range_status != 0x04) {
Stumi 1:9bc7f95c3c7d 190 status = ReadByte(address, 0x04f);
Stumi 1:9bc7f95c3c7d 191 range_status = status & 0x07;
Stumi 1:9bc7f95c3c7d 192 wait_ms(1); // (can be removed)
Stumi 1:9bc7f95c3c7d 193 }
Stumi 1:9bc7f95c3c7d 194 return 0;
Stumi 1:9bc7f95c3c7d 195 }
Stumi 1:9bc7f95c3c7d 196
Stumi 1:9bc7f95c3c7d 197 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 198 Function name: Read_range()
Stumi 1:9bc7f95c3c7d 199 Input Parameters: address - target TOF sensor address
Stumi 1:9bc7f95c3c7d 200 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Stumi 1:9bc7f95c3c7d 201 Description: allows data to be read from a tergetted address (measurement in mm)
Stumi 1:9bc7f95c3c7d 202 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 203 int cAdafruit_VL6180X::Read_Range(uint8_t address)
Stumi 1:9bc7f95c3c7d 204 {
Stumi 1:9bc7f95c3c7d 205 int range;
Stumi 1:9bc7f95c3c7d 206 range=ReadByte(address,0x062);
Stumi 1:9bc7f95c3c7d 207 return range;
Stumi 1:9bc7f95c3c7d 208 }
Stumi 1:9bc7f95c3c7d 209
Stumi 1:9bc7f95c3c7d 210 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 211 Function name: Read_range()
Stumi 1:9bc7f95c3c7d 212 Input Parameters: address - target TOF sensor address
Stumi 1:9bc7f95c3c7d 213 Output Parameters: return 0 as class defined as int (but no meaningful outputs)
Stumi 1:9bc7f95c3c7d 214 Description: allows interrupt flags to be cleared
Stumi 1:9bc7f95c3c7d 215 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 216 int cAdafruit_VL6180X::Clear_Interrupts(uint8_t address)
Stumi 1:9bc7f95c3c7d 217 {
Stumi 1:9bc7f95c3c7d 218 WriteByte(address, 0x015,0x07);
Stumi 1:9bc7f95c3c7d 219 return 0;
Stumi 1:9bc7f95c3c7d 220 }