// ---------------------------------------------------------------------------
// Created by Tim Eckel -
// Copyright 2012 License: GNU GPL v3 http://www.gnu.org/licenses/gpl-3.0.html
//
// See "BotPing.h" for purpose, syntax, version history, links, and more.
// ---------------------------------------------------------------------------
#include "BotPing.h"
// ---------------------------------------------------------------------------
// BotPing constructor
// ---------------------------------------------------------------------------
BotPing::BotPing(uint8_t trigger_pin, uint8_t echo_pin, int max_cm_distance) {
_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.
_maxEchoTime = min(max_cm_distance, MAX_SENSOR_DISTANCE) * US_ROUNDTRIP_CM + (US_ROUNDTRIP_CM / 2); // Calculate the maximum distance in uS.
#if DISABLE_ONE_PIN == true
*_triggerMode |= _triggerBit; // Set trigger pin to output.
#endif
}
// ---------------------------------------------------------------------------
// Standard ping methods
// ---------------------------------------------------------------------------
unsigned int BotPing::ping() {
if (!ping_trigger()) return NO_ECHO; // Trigger a ping, if it returns false, return NO_ECHO to the calling function.
while (*_echoInput _echoBit) // Wait for the ping echo.
if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance.
return (micros() - (_max_time - _maxEchoTime) - 5); // Calculate ping time, 5uS of overhead.
}
unsigned int BotPing::ping_in() {
unsigned int echoTime = BotPing::ping(); // Calls the ping method and returns with the ping echo distance in uS.
return BotPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
}
unsigned int BotPing::ping_cm() {
unsigned int echoTime = BotPing::ping(); // Calls the ping method and returns with the ping echo distance in uS.
return BotPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
}
unsigned int BotPing::ping_median(uint8_t it) {
unsigned int uS[it], last;
uint8_t j, i = 0;
uS[0] = NO_ECHO;
while (i 0) { // Don't start sort till second ping.
for (j = i; j > 0 uS[j - 1] > 10)); // Millisecond delay between pings.
}
return (uS[it >> 1]); // Return the ping distance median.
}
// ---------------------------------------------------------------------------
// Standard ping method support functions (not called directly)
// ---------------------------------------------------------------------------
boolean BotPing::ping_trigger() {
#if DISABLE_ONE_PIN != 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, testing shows it needs 4uS to work every time.
*_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 DISABLE_ONE_PIN != 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
_max_time = micros() + MAX_SENSOR_DELAY; // Set a timeout for the ping to trigger.
while (*_echoInput _echoBit micros() _max_time) return false; // Something went wrong, abort.
_max_time = micros() + _maxEchoTime; // Ping started, set the timeout.
return true; // Ping started successfully.
}
// ---------------------------------------------------------------------------
// Timer interrupt ping methods (won't work with ATmega8 and ATmega128)
// ---------------------------------------------------------------------------
void BotPing::ping_timer(void (*userFunc)(void)) {
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 BotPing::check_timer() {
if (micros() > _max_time) { // Outside the timeout limit.
// timer_stop(); // Disable timer interrupt
return false; // Cancel ping timer.
}
if (!(*_echoInput _echoBit)) { // Ping echo received.
// timer_stop(); // Disable timer interrupt
ping_result = (micros() - (_max_time - _maxEchoTime) - 9); // Calculate ping time, 9uS of 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;
void BotPing::timer_us(unsigned int frequency, void (*userFunc)(void)) {
timer_setup(); // Configure the timer interrupt.
intFunc = userFunc; // User's function to call when there's a timer event.
#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<#endif
}
void BotPing::timer_ms(unsigned long frequency, void (*userFunc)(void)) {
timer_setup(); // Configure the timer interrupt.
intFunc = BotPing::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.
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
OCR4C = 249; // Every count is 4uS, so 1ms = 250 counts - 1.
TIMSK4 = (1<#else
OCR2A = 249; // Every count is 4uS, so 1ms = 250 counts - 1.
TIMSK2 |= (1<#endif
}
void BotPing::timer_stop() { // Disable timer interrupt.
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
TIMSK4 = 0;
#else
TIMSK2 = ~(1<#endif
}
// ---------------------------------------------------------------------------
// Timer2/Timer4 interrupt method support functions (not called directly)
// ---------------------------------------------------------------------------
void BotPing::timer_setup() {
#if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo).
timer_stop(); // Disable Timer4 interrupt.
TCCR4A = TCCR4C = TCCR4D = TCCR4E = 0;
TCCR4B = (1< TIFR4 = (1< TCNT4 = 0; // Reset Timer4 counter.
#else
timer_stop(); // Disable Timer2 interrupt.
ASSR = ~(1< TCCR2A = (1< TCCR2B = (1< TCNT2 = 0; // Reset Timer2 counter.
#endif
}
void BotPing::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) {
#else
ISR(TIMER2_COMPA_vect) {
#endif
if(intFunc) intFunc(); // If wrapped function is set, call it.
}
// ---------------------------------------------------------------------------
// Conversion methods (rounds result to nearest inch or cm).
// ---------------------------------------------------------------------------
unsigned int BotPing::convert_in(unsigned int echoTime) {
return BotPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches.
}
unsigned int BotPing::convert_cm(unsigned int echoTime) {
return BotPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters.
}