30 lines
628 B
C++
30 lines
628 B
C++
#include "Sonar.h"
|
|
|
|
Sonar::Sonar(int trigPin, int echoPin, long maxDistCm)
|
|
: _trigPin(trigPin), _echoPin(echoPin),
|
|
_maxDistCm(maxDistCm), _lastDistance(0) {
|
|
pinMode(_trigPin, OUTPUT);
|
|
pinMode(_echoPin, INPUT);
|
|
}
|
|
|
|
long Sonar::measure() {
|
|
digitalWrite(_trigPin, LOW);
|
|
delayMicroseconds(2);
|
|
digitalWrite(_trigPin, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(_trigPin, LOW);
|
|
|
|
long duration = pulseIn(_echoPin, HIGH);
|
|
_lastDistance = duration / 58;
|
|
|
|
if (_lastDistance > _maxDistCm) {
|
|
_lastDistance = _maxDistCm;
|
|
}
|
|
|
|
return _lastDistance;
|
|
}
|
|
|
|
long Sonar::getDistance() const {
|
|
return _lastDistance;
|
|
}
|