-
Notifications
You must be signed in to change notification settings - Fork 11
/
NewPingESP8266.cpp
executable file
·214 lines (171 loc) · 10.3 KB
/
NewPingESP8266.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
// ---------------------------------------------------------------------------
// Created by Tim Eckel - teckel@leethost.com
// Copyright 2016 License: GNU GPL v3 http://www.gnu.org/licenses/gpl.html
//
// See "NewPingESP8266.h" for purpose, syntax, version history, links, and more.
// ---------------------------------------------------------------------------
#include "NewPingESP8266.h"
// ---------------------------------------------------------------------------
// NewPingESP8266 constructor
// ---------------------------------------------------------------------------
NewPingESP8266::NewPingESP8266(uint32_t trigger_pin, uint32_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 = (uint32_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
// ---------------------------------------------------------------------------
unsigned int NewPingESP8266::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 NewPingESP8266::ping_cm(unsigned int max_cm_distance) {
unsigned long echoTime = NewPingESP8266::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 NewPingESP8266Convert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}
unsigned long NewPingESP8266::ping_in(unsigned int max_cm_distance) {
unsigned long echoTime = NewPingESP8266::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 NewPingESP8266Convert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}
unsigned long NewPingESP8266::ping_median(uint32_t it, unsigned int max_cm_distance) {
unsigned int uS[it], last;
uint32_t j, i = 0;
unsigned long 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 NewPingESP8266::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 NewPingESP8266::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
}
// ---------------------------------------------------------------------------
// Conversion methods (rounds result to nearest cm or inch).
// ---------------------------------------------------------------------------
unsigned int NewPingESP8266::convert_cm(unsigned int echoTime) {
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_CM); // Convert uS to centimeters (no rounding).
#else
return NewPingESP8266Convert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
#endif
}
unsigned int NewPingESP8266::convert_in(unsigned int echoTime) {
#if ROUNDING_ENABLED == false
return (echoTime / US_ROUNDTRIP_IN); // Convert uS to inches (no rounding).
#else
return NewPingESP8266Convert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
#endif
}