Old working code
Dependencies: mbed QEI ros_lib_melodic
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 }
Generated on Tue Jul 26 2022 23:07:14 by
1.7.2