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@4:36a04230554d, 2019-11-07 (annotated)

Committer:
Stumi
Date:
Thu Nov 07 15:31:52 2019 +0000
Revision:
4:36a04230554d
Parent:
2:6197e1cf2cd1
Child:
10:276cc357015c
Added power management class and obstacle avoidance

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