User | Revision | Line number | New contents of line |
madcowswe |
0:525cb1ac9f1f
|
1
|
/* mbed SRF05 Ultrasonic Rangefiner Library
|
madcowswe |
0:525cb1ac9f1f
|
2
|
* Copyright (c) 2007-2010, cstyles, sford
|
madcowswe |
0:525cb1ac9f1f
|
3
|
*
|
madcowswe |
0:525cb1ac9f1f
|
4
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
madcowswe |
0:525cb1ac9f1f
|
5
|
* of this software and associated documentation files (the "Software"), to deal
|
madcowswe |
0:525cb1ac9f1f
|
6
|
* in the Software without restriction, including without limitation the rights
|
madcowswe |
0:525cb1ac9f1f
|
7
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
madcowswe |
0:525cb1ac9f1f
|
8
|
* copies of the Software, and to permit persons to whom the Software is
|
madcowswe |
0:525cb1ac9f1f
|
9
|
* furnished to do so, subject to the following conditions:
|
madcowswe |
0:525cb1ac9f1f
|
10
|
*
|
madcowswe |
0:525cb1ac9f1f
|
11
|
* The above copyright notice and this permission notice shall be included in
|
madcowswe |
0:525cb1ac9f1f
|
12
|
* all copies or substantial portions of the Software.
|
madcowswe |
0:525cb1ac9f1f
|
13
|
*
|
madcowswe |
0:525cb1ac9f1f
|
14
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
madcowswe |
0:525cb1ac9f1f
|
15
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
madcowswe |
0:525cb1ac9f1f
|
16
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
madcowswe |
0:525cb1ac9f1f
|
17
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
madcowswe |
0:525cb1ac9f1f
|
18
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
madcowswe |
0:525cb1ac9f1f
|
19
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
madcowswe |
0:525cb1ac9f1f
|
20
|
* THE SOFTWARE.
|
madcowswe |
0:525cb1ac9f1f
|
21
|
*/
|
madcowswe |
0:525cb1ac9f1f
|
22
|
|
madcowswe |
0:525cb1ac9f1f
|
23
|
#include "RFSRF05.h"
|
madcowswe |
0:525cb1ac9f1f
|
24
|
#include "mbed.h"
|
madcowswe |
0:525cb1ac9f1f
|
25
|
|
madcowswe |
0:525cb1ac9f1f
|
26
|
RFSRF05::RFSRF05(PinName trigger,
|
madcowswe |
0:525cb1ac9f1f
|
27
|
PinName echo0,
|
madcowswe |
0:525cb1ac9f1f
|
28
|
PinName echo1,
|
madcowswe |
0:525cb1ac9f1f
|
29
|
PinName echo2,
|
madcowswe |
0:525cb1ac9f1f
|
30
|
PinName echo3,
|
madcowswe |
0:525cb1ac9f1f
|
31
|
PinName echo4,
|
madcowswe |
0:525cb1ac9f1f
|
32
|
PinName echo5,
|
madcowswe |
0:525cb1ac9f1f
|
33
|
PinName SDI,
|
madcowswe |
0:525cb1ac9f1f
|
34
|
PinName SDO,
|
madcowswe |
0:525cb1ac9f1f
|
35
|
PinName SCK,
|
madcowswe |
0:525cb1ac9f1f
|
36
|
PinName NCS,
|
madcowswe |
0:525cb1ac9f1f
|
37
|
PinName NIRQ)
|
madcowswe |
0:525cb1ac9f1f
|
38
|
: _trigger(trigger),
|
madcowswe |
0:525cb1ac9f1f
|
39
|
_echo0(echo0),
|
madcowswe |
0:525cb1ac9f1f
|
40
|
_echo1(echo1),
|
madcowswe |
0:525cb1ac9f1f
|
41
|
_echo2(echo2),
|
madcowswe |
0:525cb1ac9f1f
|
42
|
_echo3(echo3),
|
madcowswe |
0:525cb1ac9f1f
|
43
|
_echo4(echo4),
|
madcowswe |
0:525cb1ac9f1f
|
44
|
_echo5(echo5),
|
madcowswe |
0:525cb1ac9f1f
|
45
|
_rf(SDI,SDO,SCK,NCS,NIRQ) {
|
madcowswe |
0:525cb1ac9f1f
|
46
|
|
madcowswe |
0:525cb1ac9f1f
|
47
|
|
madcowswe |
0:525cb1ac9f1f
|
48
|
// initialises codes
|
madcowswe |
0:525cb1ac9f1f
|
49
|
_code[0] = CODE0;
|
madcowswe |
0:525cb1ac9f1f
|
50
|
_code[1] = CODE1;
|
madcowswe |
0:525cb1ac9f1f
|
51
|
_code[2] = CODE2;
|
madcowswe |
0:525cb1ac9f1f
|
52
|
|
madcowswe |
0:525cb1ac9f1f
|
53
|
//set callback execute to true
|
madcowswe |
0:525cb1ac9f1f
|
54
|
ValidPulse = true;
|
madcowswe |
0:525cb1ac9f1f
|
55
|
|
madcowswe |
0:525cb1ac9f1f
|
56
|
// Attach interrupts
|
madcowswe |
0:525cb1ac9f1f
|
57
|
_echo0.rise(this, &RFSRF05::_rising);
|
madcowswe |
0:525cb1ac9f1f
|
58
|
_echo0.fall(this, &RFSRF05::_falling);
|
madcowswe |
0:525cb1ac9f1f
|
59
|
_echo1.fall(this, &RFSRF05::_falling);
|
madcowswe |
0:525cb1ac9f1f
|
60
|
_echo2.fall(this, &RFSRF05::_falling);
|
madcowswe |
0:525cb1ac9f1f
|
61
|
_echo3.fall(this, &RFSRF05::_falling);
|
madcowswe |
0:525cb1ac9f1f
|
62
|
_echo4.fall(this, &RFSRF05::_falling);
|
madcowswe |
0:525cb1ac9f1f
|
63
|
_echo5.fall(this, &RFSRF05::_falling);
|
madcowswe |
0:525cb1ac9f1f
|
64
|
|
madcowswe |
0:525cb1ac9f1f
|
65
|
//init callabck function
|
madcowswe |
0:525cb1ac9f1f
|
66
|
callbackfunc = NULL;
|
madcowswe |
0:525cb1ac9f1f
|
67
|
|
madcowswe |
0:525cb1ac9f1f
|
68
|
// innitialises beacon counter
|
madcowswe |
0:525cb1ac9f1f
|
69
|
_beacon_counter = 0;
|
madcowswe |
0:525cb1ac9f1f
|
70
|
|
madcowswe |
0:525cb1ac9f1f
|
71
|
//Interrupts every 50ms
|
madcowswe |
0:525cb1ac9f1f
|
72
|
_ticker.attach(this, &RFSRF05::_startRange, 0.05);
|
madcowswe |
0:525cb1ac9f1f
|
73
|
}
|
madcowswe |
0:525cb1ac9f1f
|
74
|
|
madcowswe |
0:525cb1ac9f1f
|
75
|
void RFSRF05::_startRange() {
|
madcowswe |
0:525cb1ac9f1f
|
76
|
|
madcowswe |
0:525cb1ac9f1f
|
77
|
// increments counter
|
madcowswe |
0:525cb1ac9f1f
|
78
|
_beacon_counter = (_beacon_counter + 1) % 3;
|
madcowswe |
0:525cb1ac9f1f
|
79
|
|
madcowswe |
0:525cb1ac9f1f
|
80
|
// writes code to RF port
|
madcowswe |
0:525cb1ac9f1f
|
81
|
_rf.write(_code[_beacon_counter]);
|
madcowswe |
0:525cb1ac9f1f
|
82
|
|
madcowswe |
0:525cb1ac9f1f
|
83
|
// send a trigger pulse, 10uS long
|
madcowswe |
0:525cb1ac9f1f
|
84
|
ValidPulse = false;
|
madcowswe |
0:525cb1ac9f1f
|
85
|
|
madcowswe |
0:525cb1ac9f1f
|
86
|
_trigger = 1;
|
madcowswe |
0:525cb1ac9f1f
|
87
|
wait_us (10);
|
madcowswe |
0:525cb1ac9f1f
|
88
|
_trigger = 0;
|
madcowswe |
0:525cb1ac9f1f
|
89
|
}
|
madcowswe |
0:525cb1ac9f1f
|
90
|
|
madcowswe |
0:525cb1ac9f1f
|
91
|
// Clear and start the timer at the begining of the echo pulse
|
madcowswe |
0:525cb1ac9f1f
|
92
|
void RFSRF05::_rising(void) {
|
madcowswe |
0:525cb1ac9f1f
|
93
|
|
madcowswe |
0:525cb1ac9f1f
|
94
|
_timer.reset();
|
madcowswe |
0:525cb1ac9f1f
|
95
|
_timer.start();
|
madcowswe |
0:525cb1ac9f1f
|
96
|
|
madcowswe |
0:525cb1ac9f1f
|
97
|
//Set callback execute to ture
|
madcowswe |
0:525cb1ac9f1f
|
98
|
ValidPulse = true;
|
madcowswe |
0:525cb1ac9f1f
|
99
|
}
|
madcowswe |
0:525cb1ac9f1f
|
100
|
|
madcowswe |
0:525cb1ac9f1f
|
101
|
// Stop and read the timer at the end of the pulse
|
madcowswe |
0:525cb1ac9f1f
|
102
|
void RFSRF05::_falling(void) {
|
madcowswe |
0:525cb1ac9f1f
|
103
|
_timer.stop();
|
madcowswe |
0:525cb1ac9f1f
|
104
|
|
madcowswe |
0:525cb1ac9f1f
|
105
|
if (ValidPulse) {
|
madcowswe |
0:525cb1ac9f1f
|
106
|
ValidPulse = false;
|
madcowswe |
0:525cb1ac9f1f
|
107
|
|
madcowswe |
0:525cb1ac9f1f
|
108
|
//Calucate distance
|
madcowswe |
0:525cb1ac9f1f
|
109
|
_dist[_beacon_counter] = _timer.read_us()/29.0;
|
madcowswe |
0:525cb1ac9f1f
|
110
|
|
madcowswe |
0:525cb1ac9f1f
|
111
|
if (callbackfunc)
|
madcowswe |
0:525cb1ac9f1f
|
112
|
(*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
|
madcowswe |
0:525cb1ac9f1f
|
113
|
|
madcowswe |
0:525cb1ac9f1f
|
114
|
}
|
madcowswe |
0:525cb1ac9f1f
|
115
|
|
madcowswe |
0:525cb1ac9f1f
|
116
|
}
|
madcowswe |
0:525cb1ac9f1f
|
117
|
|
madcowswe |
0:525cb1ac9f1f
|
118
|
float RFSRF05::read0() {
|
madcowswe |
0:525cb1ac9f1f
|
119
|
// returns distance
|
madcowswe |
0:525cb1ac9f1f
|
120
|
return (_dist[0]);
|
madcowswe |
0:525cb1ac9f1f
|
121
|
}
|
madcowswe |
0:525cb1ac9f1f
|
122
|
|
madcowswe |
0:525cb1ac9f1f
|
123
|
float RFSRF05::read1() {
|
madcowswe |
0:525cb1ac9f1f
|
124
|
// returns distance
|
madcowswe |
0:525cb1ac9f1f
|
125
|
return (_dist[1]);
|
madcowswe |
0:525cb1ac9f1f
|
126
|
}
|
madcowswe |
0:525cb1ac9f1f
|
127
|
|
madcowswe |
0:525cb1ac9f1f
|
128
|
float RFSRF05::read2() {
|
madcowswe |
0:525cb1ac9f1f
|
129
|
// returns distance
|
madcowswe |
0:525cb1ac9f1f
|
130
|
return (_dist[2]);
|
madcowswe |
0:525cb1ac9f1f
|
131
|
}
|
madcowswe |
0:525cb1ac9f1f
|
132
|
|
madcowswe |
0:525cb1ac9f1f
|
133
|
//SRF05::operator float() {
|
madcowswe |
0:525cb1ac9f1f
|
134
|
// return read();
|
madcowswe |
0:525cb1ac9f1f
|
135
|
//}
|