Old working code

Dependencies:   mbed QEI ros_lib_melodic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Sensor.cpp Source File

Sensor.cpp

00001 #include"mbed.h"
00002 #include <ros.h>
00003 
00004 #include "Sensor.h"
00005 
00006 ///////////////////////////////////////////////////////////////////
00007 // Constructor
00008 ///////////////////////////////////////////////////////////////////
00009 Sensor::Sensor(PinName sda, PinName scl, PinName shdn)
00010 : i2c(sda, scl), SHDN(shdn) 
00011 {
00012     addr = DEFAULT_DEVICE_ADDRESS << 1; 
00013     
00014     // At the beginning, turn off the device
00015     turnOff();
00016     wait_ms(0.5);
00017 }
00018 
00019 ///////////////////////////////////////////////////////////////////
00020 // Public methods
00021 ///////////////////////////////////////////////////////////////////
00022 int Sensor::init()
00023 {
00024     // Turn on the device
00025     turnOn();
00026     
00027     char reset;
00028 
00029     // check to see has it be Initialised already
00030     reset = readByte(0x016);
00031     if (reset==1) {
00032         // Mandatory settings : private registers
00033         setRegister(0x0207, 0x01);
00034         setRegister(0x0208, 0x01);
00035         setRegister(0x0096, 0x00);
00036         setRegister(0x0097, 0xfd);
00037         setRegister(0x00e3, 0x01);
00038         setRegister(0x00e4, 0x03);
00039         setRegister(0x00e5, 0x02);
00040         setRegister(0x00e6, 0x01);
00041         setRegister(0x00e7, 0x03);
00042         setRegister(0x00f5, 0x02);
00043         setRegister(0x00d9, 0x05);
00044         setRegister(0x00db, 0xce);
00045         setRegister(0x00dc, 0x03);
00046         setRegister(0x00dd, 0xf8);
00047         setRegister(0x009f, 0x00);
00048         setRegister(0x00a3, 0x3c);
00049         setRegister(0x00b7, 0x00);
00050         setRegister(0x00bb, 0x3c);
00051         setRegister(0x00b2, 0x09);
00052         setRegister(0x00ca, 0x09);
00053         setRegister(0x0198, 0x01);
00054         setRegister(0x01b0, 0x17);
00055         setRegister(0x01ad, 0x00);
00056         setRegister(0x00ff, 0x05);
00057         setRegister(0x0100, 0x05);
00058         setRegister(0x0199, 0x05);
00059         setRegister(0x01a6, 0x1b);
00060         setRegister(0x01ac, 0x3e);
00061         setRegister(0x01a7, 0x1f);
00062         setRegister(0x0030, 0x00);
00063 
00064         setRegister(SYSTEM_FRESH_OUT_OF_RESET, 0x00); //change fresh out of set status to 0
00065     } else {
00066         return -1;
00067     }
00068     return 0;
00069 }
00070 
00071 void Sensor::changeAddress(char address)
00072 {
00073     setRegister(I2C_SLAVE_DEVICE_ADDRESS, address);
00074     addr = address << 1;
00075 }
00076 
00077 float Sensor::read()
00078 {
00079     int range;
00080 
00081     // Start range measurement
00082     startRange();
00083 
00084     // Poll the VL6180 till new sample ready
00085     pollRange();
00086 
00087     // Read range result
00088     range = readRange();
00089     
00090     if (range < 255) 
00091     { 
00092         isObstacle = true;
00093     }
00094     else
00095     {
00096         isObstacle = false;
00097     }
00098 
00099     // Clear the interrupt on VL6180
00100     clearInterrupts();
00101     
00102 
00103     // Display result
00104     return range;
00105 }
00106 
00107 void Sensor::turnOff()
00108 {
00109     SHDN = 0;   
00110 }
00111 
00112 void Sensor::turnOn()
00113 {
00114     SHDN = 1;   
00115 }
00116 
00117 bool Sensor::getIsObstacle()
00118 {
00119     return isObstacle;
00120 }
00121 
00122 
00123 ///////////////////////////////////////////////////////////////////
00124 // Private methods
00125 ///////////////////////////////////////////////////////////////////
00126 void Sensor::startRange()
00127 {
00128     writeByte(0x018,0x01);
00129 }
00130 
00131 void Sensor::pollRange()
00132 {
00133     char status;
00134     char range_status;
00135 
00136     // check the status
00137     status = readByte(RESULT_INTERRUPT_STATUS_GPIO);
00138     range_status = status & 0x07;
00139 
00140     // wait for new measurement ready status
00141     while (range_status !=  0x00) {
00142         status = readByte(RESULT_INTERRUPT_STATUS_GPIO);
00143         range_status = status & 0x07;
00144     }
00145 }
00146 
00147 float Sensor::readRange()
00148 {
00149     float range;
00150     range = readByte(RESULT_RANGE_VAL);
00151     return (float)range;
00152 }
00153 
00154 void Sensor::clearInterrupts()
00155 {
00156     writeByte(0x015,0x07);
00157 }
00158 
00159 ///////////////////////////////////////////////////////////////////
00160 // Update a VL6180X register
00161 ///////////////////////////////////////////////////////////////////
00162 void Sensor::setRegister(wchar_t reg, char data)
00163 {
00164     writeByte(reg, data);
00165 }
00166 
00167 ///////////////////////////////////////////////////////////////////
00168 // Split 16-bit register address into two bytes and write
00169 // the address + data via I²C
00170 ///////////////////////////////////////////////////////////////////
00171 void Sensor::writeByte(wchar_t reg, char data)
00172 {
00173     char data_write[3];
00174     data_write[0] = (reg >> 8) & 0xFF;;
00175     // MSB of register address
00176     data_write[1] = reg  & 0xFF;
00177     // LSB of register address
00178     data_write[2] = data & 0xFF;
00179     i2c.write(addr, data_write, 3);
00180 }
00181 
00182 ///////////////////////////////////////////////////////////////////
00183 // Split 16-bit register address into two bytes and write
00184 // required register address to VL6180 and read the data back
00185 ///////////////////////////////////////////////////////////////////
00186 char Sensor::readByte(wchar_t reg)
00187 {
00188     char data_write[2];
00189     char data_read[1];
00190 
00191     data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
00192     data_write[1] = reg  & 0xFF; // LSB of register address
00193 
00194     i2c.write(addr, data_write, 2);
00195     i2c.read(addr, data_read, 1);
00196     return data_read[0];
00197 }