-
Notifications
You must be signed in to change notification settings - Fork 0
/
AmytolNewPing.cpp
357 lines (284 loc) · 16.4 KB
/
AmytolNewPing.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
// ---------------------------------------------------------------------------
// Created by Tim Eckel - [email protected]
//
// See NewPing.h for license, purpose, syntax, version history, links, etc.
// ---------------------------------------------------------------------------
#include "AmytolNewPing.h"
// ---------------------------------------------------------------------------
// NewPing constructor
// ---------------------------------------------------------------------------
NewPing::NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int 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
_one_pin_mode = (trigger_pin == echo_pin); // Automatic one pin mode detection per sensor.
set_max_distance(max_cm_distance); // Call function to set the max sensor distance.
#if (defined(__arm__) && (defined(TEENSYDUINO) || defined(PARTICLE))) || defined(ARDUINO_AVR_YUN) || 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 DO_BITWISE == true
*_triggerMode |= _triggerBit; // Set trigger pin to output.
*_triggerOutput &= ~_triggerBit; // Trigger pin should already be low, but set to low to make sure.
#else
digitalWrite(_triggerPin, LOW); // Trigger pin should already be low, but set to low to make sure.
#endif
}
// ---------------------------------------------------------------------------
// Standard ping methods
// ---------------------------------------------------------------------------
unsigned int NewPing::ping(unsigned int 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.
}
unsigned long NewPing::ping_cm(unsigned int max_cm_distance) {
unsigned long echoTime = NewPing::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 NewPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}
unsigned long NewPing::ping_in(unsigned int max_cm_distance) {
unsigned long echoTime = NewPing::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 NewPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}
unsigned long NewPing::ping_median(uint8_t it, unsigned int max_cm_distance) {
unsigned int uS[it], last;
uint8_t j, i = 0;
unsigned long t;
uS[0] = NO_ECHO;
if (max_cm_distance > 0) set_max_distance(max_cm_distance); // Call function to set a new max sensor distance.
while (i < it) {
t = micros(); // Start ping timestamp.
last = ping(); // 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()) >> 10); // 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 NewPing::ping_trigger() {
#if DO_BITWISE == true
*_triggerMode |= _triggerBit; // Set trigger pin to output (only matters if _one_pin_mode is true, but is quicker/smaller than checking _one_pin_mode state).
*_triggerOutput |= _triggerBit; // Set trigger pin high, this tells the sensor to send out a ping.
delayMicroseconds(TRIGGER_WIDTH); // Wait long enough for the sensor to realize the trigger pin is high.
*_triggerOutput &= ~_triggerBit; // Set trigger pin back to low.
if (_one_pin_mode) *_triggerMode &= ~_triggerBit; // Set trigger pin to input (this is technically setting the echo pin to input as both are tied to the same pin).
#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_mode) pinMode(_triggerPin, OUTPUT); // Set trigger pin to output.
digitalWrite(_triggerPin, HIGH); // Set trigger pin high, this tells the sensor to send out a ping.
delayMicroseconds(TRIGGER_WIDTH); // Wait long enough for the sensor to realize the trigger pin is high.
digitalWrite(_triggerPin, LOW); // Set trigger pin back to low.
if (_one_pin_mode) pinMode(_triggerPin, INPUT); // Set trigger pin to input (this is technically setting the echo pin to input as both are tied to the same pin).
#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 NewPing::set_max_distance(unsigned int max_cm_distance) {
#if ROUNDING_ENABLED == false
_maxEchoTime = min(max_cm_distance + 1, (unsigned int) MAX_SENSOR_DISTANCE + 1) * US_ROUNDTRIP_CM; // Calculate the maximum distance in uS (no rounding).
#else
_maxEchoTime = min(max_cm_distance, (unsigned int) 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 ATmega128, ATtiny and most non-AVR microcontrollers)
// ---------------------------------------------------------------------------
void NewPing::ping_timer(void (*userFunc)(void), unsigned int 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 NewPing::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)();
unsigned long _ms_cnt_reset;
volatile unsigned long _ms_cnt;
#if defined(__arm__) && (defined(TEENSYDUINO) || defined(PARTICLE))
IntervalTimer itimer;
#endif
void NewPing::timer_us(unsigned int 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.
#elif defined(__arm__) && defined(PARTICLE) // Timer for Particle devices
itimer.begin(userFunc, frequency, uSec); // Really simple on the Particle, 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 NewPing::timer_ms(unsigned long frequency, void (*userFunc)(void)) {
intFunc = NewPing::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(NewPing::timer_ms_cntdwn, 1000); // Set timer to 1ms (1000 uS).
#elif defined(__arm__) && defined(PARTICLE) // Timer for Particle
itimer.begin(NewPing::timer_ms_cntdwn, 1000, uSec); // 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 NewPing::timer_stop() { // Disable timer interrupt.
#if defined(__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
TIMSK4 = 0;
#elif defined(__arm__) && (defined(TEENSYDUINO) || defined(PARTICLE)) // Timer for Teensy 3.x & Particle
itimer.end();
#else
TIMSK2 &= ~(1<<OCIE2A);
#endif
}
// ---------------------------------------------------------------------------
// Timer2/Timer4 interrupt method support functions (not called directly)
// ---------------------------------------------------------------------------
void NewPing::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) || defined(PARTICLE))
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 NewPing::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).
// ---------------------------------------------------------------------------
unsigned int NewPing::convert_cm(unsigned int echoTime) {
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_CM); // Convert uS to centimeters (no rounding).
#else
return NewPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}
unsigned int NewPing::convert_in(unsigned int echoTime) {
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_IN); // Convert uS to inches (no rounding).
#else
return NewPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}