// --------------------------------------------------------------------------- // Created by Tim Eckel - eckel.tim@gmail.com // // See NewPing.h for license, purpose, syntax, version history, links, etc. // --------------------------------------------------------------------------- #include "NewPing.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<>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<