Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

VL6180.cpp@10:c752a8d76ee2, 2019-12-17 (annotated)

Committer:
Jonathan738
Date:
Tue Dec 17 16:33:46 2019 +0000
Revision:
10:c752a8d76ee2
Child:
11:0b9098ec11c7
Added TOF class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan738 10:c752a8d76ee2 1 #include "mbed.h"
Jonathan738 10:c752a8d76ee2 2 #include "General.hpp"
Jonathan738 10:c752a8d76ee2 3 #include "rtos.h"
Jonathan738 10:c752a8d76ee2 4 #include "Pins.h"
Jonathan738 10:c752a8d76ee2 5 #include "VL6180.hpp"
Jonathan738 10:c752a8d76ee2 6 #include <vector>
Jonathan738 10:c752a8d76ee2 7
Jonathan738 10:c752a8d76ee2 8 #define DEBUG_TOF
Jonathan738 10:c752a8d76ee2 9 #ifdef DEBUG_TOF
Jonathan738 10:c752a8d76ee2 10 Serial debug_term(SERIAL_TX, SERIAL_RX);
Jonathan738 10:c752a8d76ee2 11 #endif
Jonathan738 10:c752a8d76ee2 12
Jonathan738 10:c752a8d76ee2 13 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 14 // Main Program loop
Jonathan738 10:c752a8d76ee2 15 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 16 int TOF_Thread()
Jonathan738 10:c752a8d76ee2 17 {
Jonathan738 10:c752a8d76ee2 18 /* Pins are (PC_9, PC_11, PD_2, PG_3) */
Jonathan738 10:c752a8d76ee2 19 I2C i2c_bus(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE
Jonathan738 10:c752a8d76ee2 20
Jonathan738 10:c752a8d76ee2 21 //Dynamically create VL6180 objects for the TOF sensors
Jonathan738 10:c752a8d76ee2 22 PinName SHDN_Pins[num_VL6180] = SHDN_Pins_Cell;
Jonathan738 10:c752a8d76ee2 23 char Addresses[num_VL6180] = Shifted_TOF_Addresses;
Jonathan738 10:c752a8d76ee2 24 TOFsPtr * TOF = new TOFsPtr[num_VL6180];
Jonathan738 10:c752a8d76ee2 25 for (int idx = 0; idx < num_VL6180; ++idx) {
Jonathan738 10:c752a8d76ee2 26 TOF[idx] = new VL6180(i2c_bus, SHDN_Pins[idx], Addresses[idx]);
Jonathan738 10:c752a8d76ee2 27 }
Jonathan738 10:c752a8d76ee2 28
Jonathan738 10:c752a8d76ee2 29 // Initialise all TOF sensors
Jonathan738 10:c752a8d76ee2 30 Init_All_TOFs(TOF);
Jonathan738 10:c752a8d76ee2 31
Jonathan738 10:c752a8d76ee2 32 //Create range variable to stored read TOF values
Jonathan738 10:c752a8d76ee2 33 int Range[num_VL6180] = {};
Jonathan738 10:c752a8d76ee2 34
Jonathan738 10:c752a8d76ee2 35 //note that sensor 1 can be left at the default address, since other sensors
Jonathan738 10:c752a8d76ee2 36 //are already allocated. Adding more I2C devices would require it to be
Jonathan738 10:c752a8d76ee2 37 //reassigned to a different address, however.
Jonathan738 10:c752a8d76ee2 38 while (1) {
Jonathan738 10:c752a8d76ee2 39 for(int IDX = 0; IDX < num_VL6180; IDX++) {
Jonathan738 10:c752a8d76ee2 40 TOF[IDX]->Start_Range();
Jonathan738 10:c752a8d76ee2 41 TOF[IDX]->Poll_Range();
Jonathan738 10:c752a8d76ee2 42 Range[IDX] = TOF[IDX]->Read_Range();
Jonathan738 10:c752a8d76ee2 43 TOF[IDX]->Clear_Interrupts();
Jonathan738 10:c752a8d76ee2 44
Jonathan738 10:c752a8d76ee2 45 wait_ms(10);
Jonathan738 10:c752a8d76ee2 46 }
Jonathan738 10:c752a8d76ee2 47
Jonathan738 10:c752a8d76ee2 48 #ifdef DEBUG_TOF
Jonathan738 10:c752a8d76ee2 49 debug_term.printf("|--------------------------------\n\r"
Jonathan738 10:c752a8d76ee2 50 "|Ranges: \n\r"
Jonathan738 10:c752a8d76ee2 51 "| %d\n\r",Range[0]);
Jonathan738 10:c752a8d76ee2 52
Jonathan738 10:c752a8d76ee2 53 /*
Jonathan738 10:c752a8d76ee2 54 "| %d\n\r"
Jonathan738 10:c752a8d76ee2 55 "| %d\n\r"
Jonathan738 10:c752a8d76ee2 56 "| %d\n\r",Range[0], Range[1], Range[2], Range[3]);
Jonathan738 10:c752a8d76ee2 57 */
Jonathan738 10:c752a8d76ee2 58 #endif
Jonathan738 10:c752a8d76ee2 59
Jonathan738 10:c752a8d76ee2 60 wait_ms(100);
Jonathan738 10:c752a8d76ee2 61
Jonathan738 10:c752a8d76ee2 62 }
Jonathan738 10:c752a8d76ee2 63 }
Jonathan738 10:c752a8d76ee2 64
Jonathan738 10:c752a8d76ee2 65
Jonathan738 10:c752a8d76ee2 66 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 67 Function turns TOF sensor ON or OFF
Jonathan738 10:c752a8d76ee2 68 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 69 bool VL6180::TOF_PWR(bool State)
Jonathan738 10:c752a8d76ee2 70 {
Jonathan738 10:c752a8d76ee2 71 this->shutdown = State;
Jonathan738 10:c752a8d76ee2 72 return State;
Jonathan738 10:c752a8d76ee2 73 }
Jonathan738 10:c752a8d76ee2 74
Jonathan738 10:c752a8d76ee2 75 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 76 Split 16-bit register address into two bytes and write
Jonathan738 10:c752a8d76ee2 77 the address + data via I²C
Jonathan738 10:c752a8d76ee2 78 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 79 void VL6180::WriteByte(wchar_t reg, char data)
Jonathan738 10:c752a8d76ee2 80 {
Jonathan738 10:c752a8d76ee2 81 char data_write[3];
Jonathan738 10:c752a8d76ee2 82 data_write[0] = (reg >> 8) & 0xFF;;
Jonathan738 10:c752a8d76ee2 83 // MSB of register address
Jonathan738 10:c752a8d76ee2 84 data_write[1] = reg & 0xFF;
Jonathan738 10:c752a8d76ee2 85 // LSB of register address
Jonathan738 10:c752a8d76ee2 86 data_write[2] = data & 0xFF;
Jonathan738 10:c752a8d76ee2 87 this->i2c.write(addr, data_write, 3);
Jonathan738 10:c752a8d76ee2 88 }
Jonathan738 10:c752a8d76ee2 89 void VL6180::WriteByte_Uninitialized(wchar_t reg, char data, char Uninitialized_Address) {
Jonathan738 10:c752a8d76ee2 90 char data_write[3];
Jonathan738 10:c752a8d76ee2 91 data_write[0] = (reg >> 8) & 0xFF;;
Jonathan738 10:c752a8d76ee2 92 // MSB of register address
Jonathan738 10:c752a8d76ee2 93 data_write[1] = reg & 0xFF;
Jonathan738 10:c752a8d76ee2 94 // LSB of register address
Jonathan738 10:c752a8d76ee2 95 data_write[2] = data & 0xFF;
Jonathan738 10:c752a8d76ee2 96 this->i2c.write(Uninitialized_Address, data_write, 3);
Jonathan738 10:c752a8d76ee2 97 }
Jonathan738 10:c752a8d76ee2 98
Jonathan738 10:c752a8d76ee2 99 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 100 Split 16-bit register address into two bytes and write
Jonathan738 10:c752a8d76ee2 101 required register address to VL6180 and read the data back
Jonathan738 10:c752a8d76ee2 102 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 103 char VL6180::ReadByte(wchar_t reg)
Jonathan738 10:c752a8d76ee2 104 {
Jonathan738 10:c752a8d76ee2 105 char data_write[2];
Jonathan738 10:c752a8d76ee2 106 char data_read[1];
Jonathan738 10:c752a8d76ee2 107
Jonathan738 10:c752a8d76ee2 108 data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
Jonathan738 10:c752a8d76ee2 109 data_write[1] = reg & 0xFF; // LSB of register address
Jonathan738 10:c752a8d76ee2 110
Jonathan738 10:c752a8d76ee2 111 this->i2c.write(addr, data_write, 2);
Jonathan738 10:c752a8d76ee2 112 this->i2c.read(addr, data_read, 1);
Jonathan738 10:c752a8d76ee2 113 return data_read[0];
Jonathan738 10:c752a8d76ee2 114 }
Jonathan738 10:c752a8d76ee2 115 char VL6180::ReadByte_Uninitialized(wchar_t reg, char Uninitialized_Address) {
Jonathan738 10:c752a8d76ee2 116 char data_write[2];
Jonathan738 10:c752a8d76ee2 117 char data_read[1];
Jonathan738 10:c752a8d76ee2 118
Jonathan738 10:c752a8d76ee2 119 data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
Jonathan738 10:c752a8d76ee2 120 data_write[1] = reg & 0xFF; // LSB of register address
Jonathan738 10:c752a8d76ee2 121
Jonathan738 10:c752a8d76ee2 122 this->i2c.write(Uninitialized_Address, data_write, 2);
Jonathan738 10:c752a8d76ee2 123 this->i2c.read(Uninitialized_Address, data_read, 1);
Jonathan738 10:c752a8d76ee2 124 return data_read[0];
Jonathan738 10:c752a8d76ee2 125 }
Jonathan738 10:c752a8d76ee2 126
Jonathan738 10:c752a8d76ee2 127 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 128 Start a range measurement in single shot mode
Jonathan738 10:c752a8d76ee2 129 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 130 int VL6180::Start_Range(void)
Jonathan738 10:c752a8d76ee2 131 {
Jonathan738 10:c752a8d76ee2 132 this->WriteByte(0x018,0x03);
Jonathan738 10:c752a8d76ee2 133 return 0;
Jonathan738 10:c752a8d76ee2 134 }
Jonathan738 10:c752a8d76ee2 135
Jonathan738 10:c752a8d76ee2 136 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 137 poll for new sample ready ready
Jonathan738 10:c752a8d76ee2 138 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 139 int VL6180::Poll_Range(void)
Jonathan738 10:c752a8d76ee2 140 {
Jonathan738 10:c752a8d76ee2 141 char status;
Jonathan738 10:c752a8d76ee2 142 char range_status;
Jonathan738 10:c752a8d76ee2 143
Jonathan738 10:c752a8d76ee2 144 // check the status
Jonathan738 10:c752a8d76ee2 145 status = this->ReadByte(0x04f);
Jonathan738 10:c752a8d76ee2 146 range_status = status & 0x07;
Jonathan738 10:c752a8d76ee2 147
Jonathan738 10:c752a8d76ee2 148 // wait for new measurement ready status
Jonathan738 10:c752a8d76ee2 149 while (range_status != 0x00) {
Jonathan738 10:c752a8d76ee2 150 status = this->ReadByte(0x04f);
Jonathan738 10:c752a8d76ee2 151 range_status = status & 0x07;
Jonathan738 10:c752a8d76ee2 152 //wait_ms(50); // (can be removed)
Jonathan738 10:c752a8d76ee2 153 }
Jonathan738 10:c752a8d76ee2 154
Jonathan738 10:c752a8d76ee2 155 return 0;
Jonathan738 10:c752a8d76ee2 156 }
Jonathan738 10:c752a8d76ee2 157
Jonathan738 10:c752a8d76ee2 158
Jonathan738 10:c752a8d76ee2 159 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 160 Read range result (mm)
Jonathan738 10:c752a8d76ee2 161 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 162 int VL6180::Read_Range(void)
Jonathan738 10:c752a8d76ee2 163 {
Jonathan738 10:c752a8d76ee2 164 int range;
Jonathan738 10:c752a8d76ee2 165 range = this->ReadByte(0x062);
Jonathan738 10:c752a8d76ee2 166 return range;
Jonathan738 10:c752a8d76ee2 167 }
Jonathan738 10:c752a8d76ee2 168
Jonathan738 10:c752a8d76ee2 169 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 170 clear interrupts
Jonathan738 10:c752a8d76ee2 171 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 172
Jonathan738 10:c752a8d76ee2 173 int VL6180::Clear_Interrupts(void)
Jonathan738 10:c752a8d76ee2 174 {
Jonathan738 10:c752a8d76ee2 175 this->WriteByte(0x015,0x07);
Jonathan738 10:c752a8d76ee2 176 return 0;
Jonathan738 10:c752a8d76ee2 177 }
Jonathan738 10:c752a8d76ee2 178
Jonathan738 10:c752a8d76ee2 179 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 180 load settings
Jonathan738 10:c752a8d76ee2 181 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 182 bool VL6180::Init(void)
Jonathan738 10:c752a8d76ee2 183 {
Jonathan738 10:c752a8d76ee2 184 // check to see has if the TOF sensor has been Initialised already
Jonathan738 10:c752a8d76ee2 185 char reset = this->ReadByte_Uninitialized(0x016, 0x82);
Jonathan738 10:c752a8d76ee2 186 if (reset==1)
Jonathan738 10:c752a8d76ee2 187 {
Jonathan738 10:c752a8d76ee2 188 // Mandatory : private registers
Jonathan738 10:c752a8d76ee2 189 this->WriteByte_Uninitialized(0x0207, 0x01, 0x82);
Jonathan738 10:c752a8d76ee2 190 this->WriteByte_Uninitialized(0x0208, 0x01, 0x82);
Jonathan738 10:c752a8d76ee2 191 this->WriteByte_Uninitialized(0x0096, 0x00, 0x82);
Jonathan738 10:c752a8d76ee2 192 this->WriteByte_Uninitialized(0x0097, 0xfd, 0x82);
Jonathan738 10:c752a8d76ee2 193 this->WriteByte_Uninitialized(0x00e3, 0x01, 0x82);
Jonathan738 10:c752a8d76ee2 194 this->WriteByte_Uninitialized(0x00e4, 0x03, 0x82);
Jonathan738 10:c752a8d76ee2 195 this->WriteByte_Uninitialized(0x00e5, 0x02, 0x82);
Jonathan738 10:c752a8d76ee2 196 this->WriteByte_Uninitialized(0x00e6, 0x01, 0x82);
Jonathan738 10:c752a8d76ee2 197 this->WriteByte_Uninitialized(0x00e7, 0x03, 0x82);
Jonathan738 10:c752a8d76ee2 198 this->WriteByte_Uninitialized(0x00f5, 0x02, 0x82);
Jonathan738 10:c752a8d76ee2 199 this->WriteByte_Uninitialized(0x00d9, 0x05, 0x82);
Jonathan738 10:c752a8d76ee2 200 this->WriteByte_Uninitialized(0x00db, 0xce, 0x82);
Jonathan738 10:c752a8d76ee2 201 this->WriteByte_Uninitialized(0x00dc, 0x03, 0x82);
Jonathan738 10:c752a8d76ee2 202 this->WriteByte_Uninitialized(0x00dd, 0xf8, 0x82);
Jonathan738 10:c752a8d76ee2 203 this->WriteByte_Uninitialized(0x009f, 0x00, 0x82);
Jonathan738 10:c752a8d76ee2 204 this->WriteByte_Uninitialized(0x00a3, 0x3c, 0x82);
Jonathan738 10:c752a8d76ee2 205 this->WriteByte_Uninitialized(0x00b7, 0x00, 0x82);
Jonathan738 10:c752a8d76ee2 206 this->WriteByte_Uninitialized(0x00bb, 0x3c, 0x82);
Jonathan738 10:c752a8d76ee2 207 this->WriteByte_Uninitialized(0x00b2, 0x09, 0x82);
Jonathan738 10:c752a8d76ee2 208 this->WriteByte_Uninitialized(0x00ca, 0x09, 0x82);
Jonathan738 10:c752a8d76ee2 209 this->WriteByte_Uninitialized(0x0198, 0x01, 0x82);
Jonathan738 10:c752a8d76ee2 210 this->WriteByte_Uninitialized(0x01b0, 0x17, 0x82);
Jonathan738 10:c752a8d76ee2 211 this->WriteByte_Uninitialized(0x01ad, 0x00, 0x82);
Jonathan738 10:c752a8d76ee2 212 this->WriteByte_Uninitialized(0x00ff, 0x05, 0x82);
Jonathan738 10:c752a8d76ee2 213 this->WriteByte_Uninitialized(0x0100, 0x05, 0x82);
Jonathan738 10:c752a8d76ee2 214 this->WriteByte_Uninitialized(0x0199, 0x05, 0x82);
Jonathan738 10:c752a8d76ee2 215 this->WriteByte_Uninitialized(0x01a6, 0x1b, 0x82);
Jonathan738 10:c752a8d76ee2 216 this->WriteByte_Uninitialized(0x01ac, 0x3e, 0x82);
Jonathan738 10:c752a8d76ee2 217 this->WriteByte_Uninitialized(0x01a7, 0x1f, 0x82);
Jonathan738 10:c752a8d76ee2 218 this->WriteByte_Uninitialized(0x0030, 0x00, 0x82);
Jonathan738 10:c752a8d76ee2 219 this->WriteByte_Uninitialized(0x016, 0x00, 0x82); //change fresh out of set status to 0
Jonathan738 10:c752a8d76ee2 220 } else {
Jonathan738 10:c752a8d76ee2 221 return true;
Jonathan738 10:c752a8d76ee2 222 }
Jonathan738 10:c752a8d76ee2 223 return false;
Jonathan738 10:c752a8d76ee2 224 }
Jonathan738 10:c752a8d76ee2 225
Jonathan738 10:c752a8d76ee2 226
Jonathan738 10:c752a8d76ee2 227 /*------------------------------------------------------------------------------
Jonathan738 10:c752a8d76ee2 228 Function to Initialise all TOF sensors
Jonathan738 10:c752a8d76ee2 229 ------------------------------------------------------------------------------*/
Jonathan738 10:c752a8d76ee2 230 bool Init_All_TOFs(TOFsPtr *tof)
Jonathan738 10:c752a8d76ee2 231 {
Jonathan738 10:c752a8d76ee2 232 bool Err_Flag = false;
Jonathan738 10:c752a8d76ee2 233
Jonathan738 10:c752a8d76ee2 234 char Non_Shifted_Addresses[num_VL6180] = TOF_Addresses;
Jonathan738 10:c752a8d76ee2 235
Jonathan738 10:c752a8d76ee2 236 for(int IDX = 0; IDX < num_VL6180; IDX++)
Jonathan738 10:c752a8d76ee2 237 {
Jonathan738 10:c752a8d76ee2 238 tof[IDX]->TOF_PWR(false);
Jonathan738 10:c752a8d76ee2 239 }
Jonathan738 10:c752a8d76ee2 240
Jonathan738 10:c752a8d76ee2 241 wait_ms(0.5);
Jonathan738 10:c752a8d76ee2 242
Jonathan738 10:c752a8d76ee2 243 if(num_VL6180 > 1)
Jonathan738 10:c752a8d76ee2 244 {
Jonathan738 10:c752a8d76ee2 245 for(int IDX = 1; IDX < num_VL6180; IDX++)
Jonathan738 10:c752a8d76ee2 246 {
Jonathan738 10:c752a8d76ee2 247 tof[IDX]->TOF_PWR(true);
Jonathan738 10:c752a8d76ee2 248
Jonathan738 10:c752a8d76ee2 249 if(tof[IDX]->Init() == true)
Jonathan738 10:c752a8d76ee2 250 {
Jonathan738 10:c752a8d76ee2 251 Err_Flag = true;
Jonathan738 10:c752a8d76ee2 252 }
Jonathan738 10:c752a8d76ee2 253 tof[IDX]->WriteByte(0x212, Non_Shifted_Addresses[IDX]);
Jonathan738 10:c752a8d76ee2 254
Jonathan738 10:c752a8d76ee2 255 }
Jonathan738 10:c752a8d76ee2 256 }
Jonathan738 10:c752a8d76ee2 257
Jonathan738 10:c752a8d76ee2 258 tof[0]->TOF_PWR(true);
Jonathan738 10:c752a8d76ee2 259 tof[0]->Init();
Jonathan738 10:c752a8d76ee2 260 tof[0]->WriteByte(0x212, Non_Shifted_Addresses[0]);
Jonathan738 10:c752a8d76ee2 261
Jonathan738 10:c752a8d76ee2 262 return Err_Flag;
Jonathan738 10:c752a8d76ee2 263 }