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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
|
//------------------------------------------------------------------
// qfixSonar.h
//
// This file contains the class Sonar which represents a
// sonar modul SFH04/08.
//
// Copyright 2005 by KTB mechatronics GmbH
// Author: Florian Schrapp, Stefan Enderle
// Version 1.0
//------------------------------------------------------------------
#ifndef qfixSonar_h
#define qfixSonar_h
#include "qfix.h"
#include "qfixI2C.h"
class Sonar
{
private:
uint8_t id;
I2C i2c;
public:
/** Constructor with given ID
*/
Sonar(uint8_t ID);
/** Change logical device ID
*/
void changeID(uint8_t newID);
/** Returns the distance to the next object in millimeter
*/
int distance();
};
Sonar::Sonar(uint8_t ID) : i2c()
{
id = 112+ID;
uint8_t buf[10];
buf[0]=2; // register 2 = select range register
buf[1]=48; // set maximum range to 2107 mm (formula: range=(register*43)+43 mm)
i2c.send(id, buf, 2);
buf[0]=1; // register 1 = amplification
buf[1]=4; // improve detection along center axis
i2c.send(id, buf, 2);
}
void Sonar::changeID(uint8_t newID)
{
if(newID>=0 && newID<=15){ // only 16 IDs allowed
uint8_t buf[10];
buf[0]=0;
buf[1]=160;
i2c.send(id, buf, 2);
buf[0]=0;
buf[1]=170;
i2c.send(id, buf, 2);
buf[0]=0;
buf[1]=165;
i2c.send(id, buf, 2);
buf[0]=0;
buf[1]=(newID*2)+224;
i2c.send(id, buf, 2);
id=112+newID;
} //else error!
}
int Sonar::distance()
{
uint8_t buf[10];
buf[0]=0; // start measurement
buf[1]=81;
i2c.send(id, buf, 2);
msleep(70); // wait for echo!!! (about 65ms)
i2c.send(id, 3); // select byte to be read
i2c.get(id, buf, 1); // read first byte
uint8_t r1=buf[0];
i2c.send(id, 2); // select byte to be read
i2c.get(id, buf, 1); // read second byte
uint8_t r2=buf[0];
return ((r1+(256*r2))*10); // compute mm
}
#endif
|
