| User | Revision | Line number | New contents of line |
|---|
| narshu |
2:8aa491f77a0b
|
1
|
|
| narshu |
2:8aa491f77a0b
|
2
|
#include "RFSRF05.h"
|
| narshu |
2:8aa491f77a0b
|
3
|
#include "mbed.h"
|
| narshu |
2:8aa491f77a0b
|
4
|
#include "globals.h"
|
| narshu |
2:8aa491f77a0b
|
5
|
#include "system.h"
|
| narshu |
2:8aa491f77a0b
|
6
|
|
| narshu |
2:8aa491f77a0b
|
7
|
|
| narshu |
2:8aa491f77a0b
|
8
|
RFSRF05::RFSRF05(PinName trigger,
|
| narshu |
2:8aa491f77a0b
|
9
|
PinName echo0,
|
| narshu |
2:8aa491f77a0b
|
10
|
PinName echo1,
|
| narshu |
2:8aa491f77a0b
|
11
|
PinName echo2,
|
| narshu |
2:8aa491f77a0b
|
12
|
PinName echo3,
|
| narshu |
2:8aa491f77a0b
|
13
|
PinName echo4,
|
| narshu |
2:8aa491f77a0b
|
14
|
PinName echo5,
|
| narshu |
2:8aa491f77a0b
|
15
|
PinName SDI,
|
| narshu |
2:8aa491f77a0b
|
16
|
PinName SDO,
|
| narshu |
2:8aa491f77a0b
|
17
|
PinName SCK,
|
| narshu |
2:8aa491f77a0b
|
18
|
PinName NCS,
|
| narshu |
2:8aa491f77a0b
|
19
|
PinName NIRQ)
|
| narshu |
2:8aa491f77a0b
|
20
|
: _rf(SDI,SDO,SCK,NCS,NIRQ),
|
| narshu |
2:8aa491f77a0b
|
21
|
_trigger(trigger),
|
| narshu |
2:8aa491f77a0b
|
22
|
_echo0(echo0),
|
| narshu |
2:8aa491f77a0b
|
23
|
_echo1(echo1),
|
| narshu |
2:8aa491f77a0b
|
24
|
_echo2(echo2),
|
| narshu |
2:8aa491f77a0b
|
25
|
_echo3(echo3),
|
| narshu |
2:8aa491f77a0b
|
26
|
_echo4(echo4),
|
| narshu |
2:8aa491f77a0b
|
27
|
_echo5(echo5) {
|
| narshu |
2:8aa491f77a0b
|
28
|
|
| narshu |
2:8aa491f77a0b
|
29
|
// initialises codes
|
| narshu |
2:8aa491f77a0b
|
30
|
codes[0] = CODE0;
|
| narshu |
2:8aa491f77a0b
|
31
|
codes[1] = CODE1;
|
| narshu |
2:8aa491f77a0b
|
32
|
codes[2] = CODE2;
|
| narshu |
2:8aa491f77a0b
|
33
|
|
| narshu |
2:8aa491f77a0b
|
34
|
//set callback execute to true
|
| narshu |
2:8aa491f77a0b
|
35
|
ValidPulse = false;
|
| narshu |
2:8aa491f77a0b
|
36
|
|
| narshu |
2:8aa491f77a0b
|
37
|
// Attach interrupts
|
| narshu |
2:8aa491f77a0b
|
38
|
#ifdef SONAR_ECHO_INV
|
| narshu |
2:8aa491f77a0b
|
39
|
// inverted sonar inputs
|
| narshu |
2:8aa491f77a0b
|
40
|
_echo5.fall(this, &RFSRF05::_rising);
|
| narshu |
2:8aa491f77a0b
|
41
|
_echo0.rise(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
42
|
_echo1.rise(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
43
|
_echo2.rise(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
44
|
_echo3.rise(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
45
|
_echo4.rise(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
46
|
_echo5.rise(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
47
|
#else
|
| narshu |
2:8aa491f77a0b
|
48
|
_echo5.rise(this, &RFSRF05::_rising);
|
| narshu |
2:8aa491f77a0b
|
49
|
_echo0.fall(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
50
|
_echo1.fall(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
51
|
_echo2.fall(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
52
|
_echo3.fall(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
53
|
_echo4.fall(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
54
|
_echo5.fall(this, &RFSRF05::_falling);
|
| narshu |
2:8aa491f77a0b
|
55
|
#endif
|
| narshu |
2:8aa491f77a0b
|
56
|
|
| narshu |
2:8aa491f77a0b
|
57
|
|
| narshu |
2:8aa491f77a0b
|
58
|
//init callabck function
|
| narshu |
2:8aa491f77a0b
|
59
|
callbackfunc = NULL;
|
| narshu |
2:8aa491f77a0b
|
60
|
callbackobj = NULL;
|
| narshu |
2:8aa491f77a0b
|
61
|
mcallbackfunc = NULL;
|
| narshu |
2:8aa491f77a0b
|
62
|
|
| narshu |
2:8aa491f77a0b
|
63
|
// innitialises beacon counter
|
| narshu |
2:8aa491f77a0b
|
64
|
_beacon_counter = 0;
|
| narshu |
2:8aa491f77a0b
|
65
|
|
| narshu |
2:8aa491f77a0b
|
66
|
#ifdef PRIMARY_ROBOT
|
| narshu |
2:8aa491f77a0b
|
67
|
//Interrupts every 50ms for primary robot
|
| narshu |
2:8aa491f77a0b
|
68
|
_ticker.attach(this, &RFSRF05::_startRange, 0.05);
|
| narshu |
2:8aa491f77a0b
|
69
|
#else
|
| narshu |
2:8aa491f77a0b
|
70
|
//attach callback
|
| narshu |
2:8aa491f77a0b
|
71
|
_rf.callbackobj = (DummyCT*)this;
|
| narshu |
2:8aa491f77a0b
|
72
|
_rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange;
|
| narshu |
2:8aa491f77a0b
|
73
|
#endif
|
| narshu |
2:8aa491f77a0b
|
74
|
|
| narshu |
2:8aa491f77a0b
|
75
|
}
|
| narshu |
2:8aa491f77a0b
|
76
|
|
| narshu |
2:8aa491f77a0b
|
77
|
#ifdef PRIMARY_ROBOT
|
| narshu |
2:8aa491f77a0b
|
78
|
void RFSRF05::_startRange() {
|
| narshu |
2:8aa491f77a0b
|
79
|
|
| narshu |
2:8aa491f77a0b
|
80
|
//printf("Srange\r\r");
|
| narshu |
2:8aa491f77a0b
|
81
|
|
| narshu |
2:8aa491f77a0b
|
82
|
// increments counter
|
| narshu |
2:8aa491f77a0b
|
83
|
_beacon_counter = (_beacon_counter + 1) % 3;
|
| narshu |
2:8aa491f77a0b
|
84
|
|
| narshu |
2:8aa491f77a0b
|
85
|
|
| narshu |
2:8aa491f77a0b
|
86
|
// set flags
|
| narshu |
2:8aa491f77a0b
|
87
|
ValidPulse = false;
|
| narshu |
2:8aa491f77a0b
|
88
|
expValidPulse = true;
|
| narshu |
2:8aa491f77a0b
|
89
|
|
| narshu |
2:8aa491f77a0b
|
90
|
// writes code to RF port
|
| narshu |
2:8aa491f77a0b
|
91
|
_rf.write(codes[_beacon_counter]);
|
| narshu |
2:8aa491f77a0b
|
92
|
|
| narshu |
2:8aa491f77a0b
|
93
|
// send a trigger pulse, 10uS long
|
| narshu |
2:8aa491f77a0b
|
94
|
_trigger = 1;
|
| narshu |
2:8aa491f77a0b
|
95
|
wait_us (10);
|
| narshu |
2:8aa491f77a0b
|
96
|
_trigger = 0;
|
| narshu |
2:8aa491f77a0b
|
97
|
|
| narshu |
2:8aa491f77a0b
|
98
|
}
|
| narshu |
2:8aa491f77a0b
|
99
|
#else
|
| narshu |
2:8aa491f77a0b
|
100
|
|
| narshu |
2:8aa491f77a0b
|
101
|
void RFSRF05::startRange(unsigned char rx_code) {
|
| narshu |
2:8aa491f77a0b
|
102
|
for (int i = 0; i < 3; i++) {
|
| narshu |
2:8aa491f77a0b
|
103
|
if (rx_code == codes[i]) {
|
| narshu |
2:8aa491f77a0b
|
104
|
|
| narshu |
2:8aa491f77a0b
|
105
|
// assign beacon_counter
|
| narshu |
2:8aa491f77a0b
|
106
|
_beacon_counter = i;
|
| narshu |
2:8aa491f77a0b
|
107
|
|
| narshu |
2:8aa491f77a0b
|
108
|
// set flags
|
| narshu |
2:8aa491f77a0b
|
109
|
ValidPulse = false;
|
| narshu |
2:8aa491f77a0b
|
110
|
expValidPulse = true;
|
| narshu |
2:8aa491f77a0b
|
111
|
|
| narshu |
2:8aa491f77a0b
|
112
|
// send a trigger pulse, 10uS long
|
| narshu |
2:8aa491f77a0b
|
113
|
_trigger = 1;
|
| narshu |
2:8aa491f77a0b
|
114
|
wait_us (10);
|
| narshu |
2:8aa491f77a0b
|
115
|
_trigger = 0;
|
| narshu |
2:8aa491f77a0b
|
116
|
break;
|
| narshu |
2:8aa491f77a0b
|
117
|
}
|
| narshu |
2:8aa491f77a0b
|
118
|
}
|
| narshu |
2:8aa491f77a0b
|
119
|
}
|
| narshu |
2:8aa491f77a0b
|
120
|
#endif
|
| narshu |
2:8aa491f77a0b
|
121
|
|
| narshu |
2:8aa491f77a0b
|
122
|
// Clear and start the timer at the begining of the echo pulse
|
| narshu |
2:8aa491f77a0b
|
123
|
void RFSRF05::_rising(void) {
|
| narshu |
2:8aa491f77a0b
|
124
|
|
| narshu |
2:8aa491f77a0b
|
125
|
_timer.reset();
|
| narshu |
2:8aa491f77a0b
|
126
|
_timer.start();
|
| narshu |
2:8aa491f77a0b
|
127
|
|
| narshu |
2:8aa491f77a0b
|
128
|
//Set callback execute to ture
|
| narshu |
2:8aa491f77a0b
|
129
|
if (expValidPulse) {
|
| narshu |
2:8aa491f77a0b
|
130
|
ValidPulse = true;
|
| narshu |
2:8aa491f77a0b
|
131
|
expValidPulse = false;
|
| narshu |
2:8aa491f77a0b
|
132
|
}
|
| narshu |
2:8aa491f77a0b
|
133
|
}
|
| narshu |
2:8aa491f77a0b
|
134
|
|
| narshu |
2:8aa491f77a0b
|
135
|
// Stop and read the timer at the end of the pulse
|
| narshu |
2:8aa491f77a0b
|
136
|
void RFSRF05::_falling(void) {
|
| narshu |
2:8aa491f77a0b
|
137
|
_timer.stop();
|
| narshu |
2:8aa491f77a0b
|
138
|
|
| narshu |
2:8aa491f77a0b
|
139
|
if (ValidPulse) {
|
| narshu |
2:8aa491f77a0b
|
140
|
//printf("Validpulse trig!\r\n");
|
| narshu |
2:8aa491f77a0b
|
141
|
ValidPulse = false;
|
| narshu |
2:8aa491f77a0b
|
142
|
|
| narshu |
2:8aa491f77a0b
|
143
|
//Calucate distance
|
| narshu |
2:8aa491f77a0b
|
144
|
_dist[_beacon_counter] = _timer.read_us()/2.9 + 300;
|
| narshu |
2:8aa491f77a0b
|
145
|
|
| narshu |
2:8aa491f77a0b
|
146
|
if (callbackfunc)
|
| narshu |
2:8aa491f77a0b
|
147
|
(*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
|
| narshu |
2:8aa491f77a0b
|
148
|
|
| narshu |
2:8aa491f77a0b
|
149
|
if (callbackobj && mcallbackfunc)
|
| narshu |
2:8aa491f77a0b
|
150
|
(callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
|
| narshu |
2:8aa491f77a0b
|
151
|
|
| narshu |
2:8aa491f77a0b
|
152
|
}
|
| narshu |
2:8aa491f77a0b
|
153
|
|
| narshu |
2:8aa491f77a0b
|
154
|
}
|
| narshu |
2:8aa491f77a0b
|
155
|
|
| narshu |
2:8aa491f77a0b
|
156
|
float RFSRF05::read0() {
|
| narshu |
2:8aa491f77a0b
|
157
|
// returns distance
|
| narshu |
2:8aa491f77a0b
|
158
|
return (_dist[0]);
|
| narshu |
2:8aa491f77a0b
|
159
|
}
|
| narshu |
2:8aa491f77a0b
|
160
|
|
| narshu |
2:8aa491f77a0b
|
161
|
float RFSRF05::read1() {
|
| narshu |
2:8aa491f77a0b
|
162
|
// returns distance
|
| narshu |
2:8aa491f77a0b
|
163
|
return (_dist[1]);
|
| narshu |
2:8aa491f77a0b
|
164
|
}
|
| narshu |
2:8aa491f77a0b
|
165
|
|
| narshu |
2:8aa491f77a0b
|
166
|
float RFSRF05::read2() {
|
| narshu |
2:8aa491f77a0b
|
167
|
// returns distance
|
| narshu |
2:8aa491f77a0b
|
168
|
return (_dist[2]);
|
| narshu |
2:8aa491f77a0b
|
169
|
}
|
| narshu |
2:8aa491f77a0b
|
170
|
|
| narshu |
2:8aa491f77a0b
|
171
|
float RFSRF05::read(unsigned int beaconnum) {
|
| narshu |
2:8aa491f77a0b
|
172
|
// returns distance
|
| narshu |
2:8aa491f77a0b
|
173
|
return (_dist[beaconnum]);
|
| narshu |
2:8aa491f77a0b
|
174
|
}
|
| narshu |
2:8aa491f77a0b
|
175
|
|
| narshu |
2:8aa491f77a0b
|
176
|
void RFSRF05::setCode(int code_index, unsigned char code) {
|
| narshu |
2:8aa491f77a0b
|
177
|
codes[code_index] = code;
|
| narshu |
2:8aa491f77a0b
|
178
|
}
|
| narshu |
2:8aa491f77a0b
|
179
|
|
| narshu |
2:8aa491f77a0b
|
180
|
//SRF05::operator float() {
|
| narshu |
2:8aa491f77a0b
|
181
|
// return read();
|
| narshu |
2:8aa491f77a0b
|
182
|
//}
|
Shuto Naruse