-
Notifications
You must be signed in to change notification settings - Fork 0
/
HCSR04_herbk.cpp
360 lines (288 loc) · 16.4 KB
/
HCSR04_herbk.cpp
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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
/*
HCSR04_herbk library is forked from NewPing 1.8, because Newping above 1.8 is licenced with to much limits.
License of HCSR04_herbk library: GNU GPL v3 http://www.gnu.org/licenses/gpl.html
See "HCSR04_herbk.h" for purpose, syntax, version history, links, and more.
*/
#include "HCSR04_herbk.h"
// HCSR04_herbk constructor
HCSR04_herbk::HCSR04_herbk(uint8_t trigger_pin, uint8_t echo_pin, uint16_t max_cm_distance) {
#if DO_BITWISE == true
_triggerBit = digitalPinToBitMask(trigger_pin); // Get the port register bitmask for the trigger pin.
_echoBit = digitalPinToBitMask(echo_pin); // Get the port register bitmask for the echo pin.
_triggerOutput = portOutputRegister(digitalPinToPort(trigger_pin)); // Get the output port register for the trigger pin.
_echoInput = portInputRegister(digitalPinToPort(echo_pin)); // Get the input port register for the echo pin.
_triggerMode = (uint8_t *) portModeRegister(digitalPinToPort(trigger_pin)); // Get the port mode register for the trigger pin.
#else
_triggerPin = trigger_pin;
_echoPin = echo_pin;
#endif
set_max_distance(max_cm_distance); // Call function to set the max sensor distance.
#if (defined (__arm__) && defined (TEENSYDUINO)) || DO_BITWISE != true
pinMode(echo_pin, INPUT); // Set echo pin to input (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
pinMode(trigger_pin, OUTPUT); // Set trigger pin to output (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
#endif
#if defined (ARDUINO_AVR_YUN)
pinMode(echo_pin, INPUT); // Set echo pin to input for the Arduino Yun, not sure why it doesn't default this way.
#endif
#if ONE_PIN_ENABLED != true && DO_BITWISE == true
*_triggerMode |= _triggerBit; // Set trigger pin to output.
#endif
}
// ---------------------------------------------------------------------------
// Standard ping methods
// ---------------------------------------------------------------------------
uint16_t HCSR04_herbk::ping(uint16_t max_cm_distance) {
if (max_cm_distance > 0) set_max_distance(max_cm_distance); // Call function to set a new max sensor distance.
if (!ping_trigger()) return NO_ECHO; // Trigger a ping, if it returns false, return NO_ECHO to the calling function.
#if URM37_ENABLED == true
#if DO_BITWISE == true
while (!(*_echoInput & _echoBit)) // Wait for the ping echo.
#else
while (!digitalRead(_echoPin)) // Wait for the ping echo.
#endif
if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance.
#else
#if DO_BITWISE == true
while (*_echoInput & _echoBit) // Wait for the ping echo.
#else
while (digitalRead(_echoPin)) // Wait for the ping echo.
#endif
if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance.
#endif
return (micros() - (_max_time - _maxEchoTime) - PING_OVERHEAD); // Calculate ping time, include overhead.
}
uint32_t HCSR04_herbk::ping_cm(uint16_t max_cm_distance) {
uint32_t echoTime = HCSR04_herbk::ping(max_cm_distance); // Calls the ping method and returns with the ping echo distance in uS.
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_CM); // Call the ping method and returns the distance in centimeters (no rounding).
#else
return HCSR04_herbkConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}
uint32_t HCSR04_herbk::ping_in(uint16_t max_cm_distance) {
uint32_t echoTime = HCSR04_herbk::ping(max_cm_distance); // Calls the ping method and returns with the ping echo distance in uS.
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_IN); // Call the ping method and returns the distance in inches (no rounding).
#else
return HCSR04_herbkConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}
uint32_t HCSR04_herbk::ping_median(uint8_t it, uint16_t max_cm_distance) {
uint16_t uS[it], last;
uint8_t j, i = 0;
uint32_t t;
uS[0] = NO_ECHO;
while (i < it) {
t = micros(); // Start ping timestamp.
last = ping(max_cm_distance); // Send ping.
if (last != NO_ECHO) { // Ping in range, include as part of median.
if (i > 0) { // Don't start sort till second ping.
for (j = i; j > 0 && uS[j - 1] < last; j--) // Insertion sort loop.
uS[j] = uS[j - 1]; // Shift ping array to correct position for sort insertion.
} else j = 0; // First ping is sort starting point.
uS[j] = last; // Add last ping to array in sorted position.
i++; // Move to next ping.
} else it--; // Ping out of range, skip and don't include as part of median.
if (i < it && micros() - t < PING_MEDIAN_DELAY)
delay((PING_MEDIAN_DELAY + t - micros()) / 1000); // Millisecond delay between pings.
}
return (uS[it >> 1]); // Return the ping distance median.
}
// ---------------------------------------------------------------------------
// Standard and timer interrupt ping method support functions (not called directly)
// ---------------------------------------------------------------------------
boolean HCSR04_herbk::ping_trigger() {
#if DO_BITWISE == true
#if ONE_PIN_ENABLED == true
*_triggerMode |= _triggerBit; // Set trigger pin to output.
#endif
*_triggerOutput &= ~_triggerBit; // Set the trigger pin low, should already be low, but this will make sure it is.
delayMicroseconds(4); // Wait for pin to go low.
*_triggerOutput |= _triggerBit; // Set trigger pin high, this tells the sensor to send out a ping.
delayMicroseconds(10); // Wait long enough for the sensor to realize the trigger pin is high. Sensor specs say to wait 10uS.
*_triggerOutput &= ~_triggerBit; // Set trigger pin back to low.
#if ONE_PIN_ENABLED == true
*_triggerMode &= ~_triggerBit; // Set trigger pin to input (when using one Arduino pin, this is technically setting the echo pin to input as both are tied to the same Arduino pin).
#endif
#if URM37_ENABLED == true
if (!(*_echoInput & _echoBit)) return false; // Previous ping hasn't finished, abort.
_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
while (*_echoInput & _echoBit) // Wait for ping to start.
if (micros() > _max_time) return false; // Took too long to start, abort.
#else
if (*_echoInput & _echoBit) return false; // Previous ping hasn't finished, abort.
_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
while (!(*_echoInput & _echoBit)) // Wait for ping to start.
if (micros() > _max_time) return false; // Took too long to start, abort.
#endif
#else
#if ONE_PIN_ENABLED == true
pinMode(_triggerPin, OUTPUT); // Set trigger pin to output.
#endif
digitalWrite(_triggerPin, LOW); // Set the trigger pin low, should already be low, but this will make sure it is.
delayMicroseconds(4); // Wait for pin to go low.
digitalWrite(_triggerPin, HIGH); // Set trigger pin high, this tells the sensor to send out a ping.
delayMicroseconds(10); // Wait long enough for the sensor to realize the trigger pin is high. Sensor specs say to wait 10uS.
digitalWrite(_triggerPin, LOW); // Set trigger pin back to low.
#if ONE_PIN_ENABLED == true
pinMode(_triggerPin, INPUT); // Set trigger pin to input (when using one Arduino pin, this is technically setting the echo pin to input as both are tied to the same Arduino pin).
#endif
#if URM37_ENABLED == true
if (!digitalRead(_echoPin)) return false; // Previous ping hasn't finished, abort.
_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
while (digitalRead(_echoPin)) // Wait for ping to start.
if (micros() > _max_time) return false; // Took too long to start, abort.
#else
if (digitalRead(_echoPin)) return false; // Previous ping hasn't finished, abort.
_max_time = micros() + _maxEchoTime + MAX_SENSOR_DELAY; // Maximum time we'll wait for ping to start (most sensors are <450uS, the SRF06 can take up to 34,300uS!)
while (!digitalRead(_echoPin)) // Wait for ping to start.
if (micros() > _max_time) return false; // Took too long to start, abort.
#endif
#endif
_max_time = micros() + _maxEchoTime; // Ping started, set the time-out.
return true; // Ping started successfully.
}
void HCSR04_herbk::set_max_distance(uint16_t max_cm_distance) {
#if ROUNDING_ENABLED == false
_maxEchoTime = min(max_cm_distance + 1, (uint16_t) MAX_SENSOR_DISTANCE + 1) * US_ROUNDTRIP_CM; // Calculate the maximum distance in uS (no rounding).
#else
_maxEchoTime = min(max_cm_distance, (uint16_t) MAX_SENSOR_DISTANCE) * US_ROUNDTRIP_CM + (US_ROUNDTRIP_CM / 2); // Calculate the maximum distance in uS.
#endif
}
#if TIMER_ENABLED == true && DO_BITWISE == true
// ---------------------------------------------------------------------------
// Timer interrupt ping methods (won't work with non-AVR, ATmega128 and all ATtiny microcontrollers)
// ---------------------------------------------------------------------------
void HCSR04_herbk::ping_timer(void (*userFunc)(void), uint16_t max_cm_distance) {
if (max_cm_distance > 0) set_max_distance(max_cm_distance); // Call function to set a new max sensor distance.
if (!ping_trigger()) return; // Trigger a ping, if it returns false, return without starting the echo timer.
timer_us(ECHO_TIMER_FREQ, userFunc); // Set ping echo timer check every ECHO_TIMER_FREQ uS.
}
boolean HCSR04_herbk::check_timer() {
if (micros() > _max_time) { // Outside the time-out limit.
timer_stop(); // Disable timer interrupt
return false; // Cancel ping timer.
}
#if URM37_ENABLED == false
if (!(*_echoInput & _echoBit)) { // Ping echo received.
#else
if (*_echoInput & _echoBit) { // Ping echo received.
#endif
timer_stop(); // Disable timer interrupt
ping_result = (micros() - (_max_time - _maxEchoTime) - PING_TIMER_OVERHEAD); // Calculate ping time including overhead.
return true; // Return ping echo true.
}
return false; // Return false because there's no ping echo yet.
}
// ---------------------------------------------------------------------------
// Timer2/Timer4 interrupt methods (can be used for non-ultrasonic needs)
// ---------------------------------------------------------------------------
// Variables used for timer functions
void (*intFunc)();
void (*intFunc2)();
uint32_t _ms_cnt_reset;
volatile uint32_t _ms_cnt;
#if defined(__arm__) && defined(TEENSYDUINO)
IntervalTimer itimer;
#endif
void HCSR04_herbk::timer_us(uint16_t frequency, void (*userFunc)(void)) {
intFunc = userFunc; // User's function to call when there's a timer event.
timer_setup(); // Configure the timer interrupt.
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
OCR4C = min((frequency>>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit.
TIMSK4 = (1<<TOIE4); // Enable Timer4 interrupt.
#elif defined (__arm__) && defined (TEENSYDUINO) // Timer for Teensy 3.x
itimer.begin(userFunc, frequency); // Really simple on the Teensy 3.x, calls userFunc every 'frequency' uS.
#else
OCR2A = min((frequency>>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit.
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 interrupt.
#endif
}
void HCSR04_herbk::timer_ms(uint32_t frequency, void (*userFunc)(void)) {
intFunc = HCSR04_herbk::timer_ms_cntdwn; // Timer events are sent here once every ms till user's frequency is reached.
intFunc2 = userFunc; // User's function to call when user's frequency is reached.
_ms_cnt = _ms_cnt_reset = frequency; // Current ms counter and reset value.
timer_setup(); // Configure the timer interrupt.
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
OCR4C = 249; // Every count is 4uS, so 1ms = 250 counts - 1.
TIMSK4 = (1<<TOIE4); // Enable Timer4 interrupt.
#elif defined (__arm__) && defined (TEENSYDUINO) // Timer for Teensy 3.x
itimer.begin(HCSR04_herbk::timer_ms_cntdwn, 1000); // Set timer to 1ms (1000 uS).
#else
OCR2A = 249; // Every count is 4uS, so 1ms = 250 counts - 1.
TIMSK2 |= (1<<OCIE2A); // Enable Timer2 interrupt.
#endif
}
void HCSR04_herbk::timer_stop() { // Disable timer interrupt.
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
TIMSK4 = 0;
#elif defined (__arm__) && defined (TEENSYDUINO) // Timer for Teensy 3.x
itimer.end();
#else
TIMSK2 &= ~(1<<OCIE2A);
#endif
}
// ---------------------------------------------------------------------------
// Timer2/Timer4 interrupt method support functions (not called directly)
// ---------------------------------------------------------------------------
void HCSR04_herbk::timer_setup() {
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
timer_stop(); // Disable Timer4 interrupt.
TCCR4A = TCCR4C = TCCR4D = TCCR4E = 0;
TCCR4B = (1<<CS42) | (1<<CS41) | (1<<CS40) | (1<<PSR4); // Set Timer4 prescaler to 64 (4uS/count, 4uS-1020uS range).
TIFR4 = (1<<TOV4);
TCNT4 = 0; // Reset Timer4 counter.
#elif defined (__AVR_ATmega8__) || defined (__AVR_ATmega16__) || defined (__AVR_ATmega32__) || defined (__AVR_ATmega8535__) // Alternate timer commands for certain microcontrollers.
timer_stop(); // Disable Timer2 interrupt.
ASSR &= ~(1<<AS2); // Set clock, not pin.
TCCR2 = (1<<WGM21 | 1<<CS22); // Set Timer2 to CTC mode, prescaler to 64 (4uS/count, 4uS-1020uS range).
TCNT2 = 0; // Reset Timer2 counter.
#elif defined (__arm__) && defined (TEENSYDUINO)
timer_stop(); // Stop the timer.
#else
timer_stop(); // Disable Timer2 interrupt.
ASSR &= ~(1<<AS2); // Set clock, not pin.
TCCR2A = (1<<WGM21); // Set Timer2 to CTC mode.
TCCR2B = (1<<CS22); // Set Timer2 prescaler to 64 (4uS/count, 4uS-1020uS range).
TCNT2 = 0; // Reset Timer2 counter.
#endif
}
void HCSR04_herbk::timer_ms_cntdwn() {
if (!_ms_cnt--) { // Count down till we reach zero.
intFunc2(); // Scheduled time reached, run the main timer event function.
_ms_cnt = _ms_cnt_reset; // Reset the ms timer.
}
}
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
ISR(TIMER4_OVF_vect) {
intFunc(); // Call wrapped function.
}
#elif defined (__AVR_ATmega8__) || defined (__AVR_ATmega16__) || defined (__AVR_ATmega32__) || defined (__AVR_ATmega8535__) // Alternate timer commands for certain microcontrollers.
ISR(TIMER2_COMP_vect) {
intFunc(); // Call wrapped function.
}
#elif defined (__arm__)
// Do nothing...
#else
ISR(TIMER2_COMPA_vect) {
intFunc(); // Call wrapped function.
}
#endif
#endif
// ---------------------------------------------------------------------------
// Conversion methods (rounds result to nearest cm or inch).
// ---------------------------------------------------------------------------
uint16_t HCSR04_herbk::convert_cm(uint16_t echoTime) {
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_CM); // Convert uS to centimeters (no rounding).
#else
return HCSR04_herbkConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}
uint16_t HCSR04_herbk::convert_in(uint16_t echoTime) {
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_IN); // Convert uS to inches (no rounding).
#else
return HCSR04_herbkConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}