72 lines
1 KiB
C++
72 lines
1 KiB
C++
#include "Srf10.h"
|
|
|
|
#include "i2c.h"
|
|
#include "timer.h"
|
|
|
|
|
|
Srf10::Srf10(uint8_t id)
|
|
{
|
|
this->id = id;
|
|
|
|
gain = 16;
|
|
range = 255;
|
|
|
|
unit = Inches;
|
|
|
|
distance = 0;
|
|
has_distance = false;
|
|
|
|
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};
|
|
uint16_t d;
|
|
|
|
|
|
if(!I2CSend(id, data, 2)) return -1;
|
|
|
|
do {
|
|
sleep(1);
|
|
} while(readFirmware() == 0xFF);
|
|
|
|
if(!I2CSendByte(id, 2)) return -1;
|
|
if(I2CRecv(id, data, 2) < 2) return -1;
|
|
|
|
d = (((uint16_t)data[0]) << 8) | data[1];
|
|
|
|
if(d == 0) has_distance = false;
|
|
else {
|
|
has_distance = true;
|
|
distance = d;
|
|
}
|
|
|
|
return d;
|
|
}
|