Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

VL6180.cpp@11:0b9098ec11c7, 2019-12-19 (annotated)

Committer:
Jonathan738
Date:
Thu Dec 19 00:13:38 2019 +0000
Revision:
11:0b9098ec11c7
Parent:
10:c752a8d76ee2
Child:
12:82b8fe254222
Fixed several bugs, added status information and TOF data to ROS Topics

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