root / quad1 / AeroQuad / Sonar.h @ 9240aaa3
History | View | Annotate | Download (1.27 KB)
1 | 9240aaa3 | Alex | class Sonar |
---|---|---|---|
2 | { |
||
3 | public:
|
||
4 | int range;
|
||
5 | int addr;
|
||
6 | |||
7 | Sonar() |
||
8 | { |
||
9 | } |
||
10 | |||
11 | void initialize(int address) |
||
12 | { |
||
13 | //addr=ADDRESS&(0x7F); //Wire only wants 7-bit addresses, chop off the highest bit
|
||
14 | addr=address; |
||
15 | // Serial.print("Initing Sonar at 0x");
|
||
16 | // Serial.println(addr, HEX);
|
||
17 | requestRead(); |
||
18 | delay(70);
|
||
19 | |||
20 | // Serial.println("Done initing sonar");
|
||
21 | } |
||
22 | |||
23 | int getValue()
|
||
24 | { |
||
25 | return range;
|
||
26 | } |
||
27 | |||
28 | void measure()
|
||
29 | { |
||
30 | int temp = readData();
|
||
31 | range = temp==0 ? range:temp;
|
||
32 | requestRead(); |
||
33 | } |
||
34 | |||
35 | |||
36 | private:
|
||
37 | void requestRead();
|
||
38 | int readData();
|
||
39 | }; |
||
40 | |||
41 | int Sonar::readData()
|
||
42 | { |
||
43 | // Serial.print("readData() called, addr: 0x");
|
||
44 | // Serial.println(addr, HEX);
|
||
45 | Wire.beginTransmission(addr); |
||
46 | Wire.send(0x02);
|
||
47 | Wire.endTransmission(); |
||
48 | |||
49 | Wire.requestFrom(addr, 2);
|
||
50 | //while(Wire.available() < 2){};
|
||
51 | //Serial.println("Stopped Failing");
|
||
52 | int result = (Wire.receive()<<8); |
||
53 | result += Wire.receive(); |
||
54 | |||
55 | return result;
|
||
56 | } |
||
57 | |||
58 | void Sonar::requestRead()
|
||
59 | { |
||
60 | Wire.beginTransmission(addr); |
||
61 | Wire.send(0x00);
|
||
62 | Wire.send(0x50);
|
||
63 | Wire.endTransmission(); |
||
64 | } |