59 lines
963 B
C++
59 lines
963 B
C++
|
#include "hcsr04.h"
|
||
|
|
#include "median.h"
|
||
|
|
#include <wiringPi.h>
|
||
|
|
|
||
|
|
volatile unsigned int timestampHigh = 0;
|
||
|
|
volatile unsigned int pulseLength = 0;
|
||
|
|
|
||
|
|
HCSR04::HCSR04(unsigned int triggerPin, unsigned int echoPin)
|
||
|
|
: m_triggerPin(triggerPin)
|
||
|
|
, m_echoPin(echoPin)
|
||
|
|
{
|
||
|
|
wiringPiSetup();
|
||
|
|
pinMode(m_triggerPin, OUTPUT);
|
||
|
|
pinMode(m_echoPin, INPUT);
|
||
|
|
}
|
||
|
|
|
||
|
|
void HCSR04::run()
|
||
|
|
{
|
||
|
|
wiringPiISR(m_echoPin, INT_EDGE_BOTH, []
|
||
|
|
{
|
||
|
|
if (digitalRead(29) == HIGH)
|
||
|
|
{
|
||
|
|
timestampHigh = micros();
|
||
|
|
}
|
||
|
|
else if (timestampHigh != 0)
|
||
|
|
{
|
||
|
|
pulseLength = micros() - timestampHigh;
|
||
|
|
timestampHigh = 0;
|
||
|
|
}
|
||
|
|
});
|
||
|
|
}
|
||
|
|
|
||
|
|
double HCSR04::getDistance()
|
||
|
|
{
|
||
|
|
std::vector<double> values;
|
||
|
|
for (int i = 0; i < 15; ++i)
|
||
|
|
{
|
||
|
|
digitalWrite(m_triggerPin, HIGH);
|
||
|
|
delayMicroseconds(10);
|
||
|
|
digitalWrite(m_triggerPin, LOW);
|
||
|
|
delayMicroseconds(40);
|
||
|
|
|
||
|
|
if (pulseLength < 25e3)
|
||
|
|
{
|
||
|
|
values.push_back(pulseLength);
|
||
|
|
}
|
||
|
|
|
||
|
|
delay(10);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!values.empty())
|
||
|
|
{
|
||
|
|
return median(values) / 0.58;
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|