1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
|
#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;
}
|
