61 lines
904 B
C++
61 lines
904 B
C++
|
#include "Srf10.h"
|
||
|
|
|
||
|
|
#include "i2c.h"
|
||
|
|
|
||
|
|
|
||
|
|
Srf10::Srf10(uint8_t id)
|
||
|
|
{
|
||
|
|
this->id = id;
|
||
|
|
|
||
|
|
gain = 16;
|
||
|
|
range = 255;
|
||
|
|
|
||
|
|
unit = Inches;
|
||
|
|
|
||
|
|
distance = 0;
|
||
|
|
|
||
|
|
firmware = readFirmware();
|
||
|
|
}
|
||
|
|
|
||
|
|
int Srf10::readFirmware() {
|
||
|
|
uint8_t byte;
|
||
|
|
|
||
|
|
if(!I2CSendByte(id, 0)) return -1;
|
||
|
|
if(!I2CRecvByte(id, &byte)) return -1;
|
||
|
|
|
||
|
|
return byte;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool Srf10::setGain(uint8_t gain) {
|
||
|
|
uint8_t data[2] = {1, gain};
|
||
|
|
|
||
|
|
if(!I2CSend(id, data, 2)) return false;
|
||
|
|
|
||
|
|
this->gain = gain;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool Srf10::setRange(uint8_t range) {
|
||
|
|
uint8_t data[2] = {2, range};
|
||
|
|
|
||
|
|
if(!I2CSend(id, data, 2)) return false;
|
||
|
|
|
||
|
|
this->range = range;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
long Srf10::updateDistance() {
|
||
|
|
uint8_t data[2] = {0, unit};
|
||
|
|
|
||
|
|
if(!I2CSend(id, data, 2)) return -1;
|
||
|
|
|
||
|
|
while(readFirmware() != 0xFF);
|
||
|
|
|
||
|
|
if(!I2CSendByte(id, 2)) return -1;
|
||
|
|
if(!I2CRecv(id, data, 2)) return -1;
|
||
|
|
|
||
|
|
distance = (((uint16_t)data[0]) << 8) | data[1];
|
||
|
|
|
||
|
|
return distance;
|
||
|
|
}
|