89 lines
2.8 KiB
C++
89 lines
2.8 KiB
C++
#include "UltrasonicSensor.h"
|
|
|
|
#include <Arduino.h>
|
|
#include <iostream>
|
|
|
|
namespace RoboidControl {
|
|
namespace Arduino {
|
|
|
|
UltrasonicSensor::UltrasonicSensor(Configuration config,
|
|
Participant* participant)
|
|
: Thing(participant) {
|
|
this->pinTrigger = config.triggerPin;
|
|
this->pinEcho = config.echoPin;
|
|
|
|
pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode
|
|
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
|
|
}
|
|
|
|
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
|
|
: UltrasonicSensor(config, parent->owner) {
|
|
this->SetParent(parent);
|
|
}
|
|
|
|
float UltrasonicSensor::GetDistance() {
|
|
// Start the ultrasonic 'ping'
|
|
digitalWrite(pinTrigger, LOW);
|
|
delayMicroseconds(2);
|
|
digitalWrite(pinTrigger, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(pinTrigger, LOW);
|
|
|
|
// Measure the duration of the pulse on the echo pin
|
|
unsigned long duration_us =
|
|
pulseIn(pinEcho, HIGH); // the result is in microseconds
|
|
|
|
// Calculate the distance:
|
|
// * Duration should be divided by 2, because the ping goes to the object
|
|
// and back again. The distance to the object is therefore half the duration
|
|
// of the pulse: duration_us /= 2;
|
|
// * Then we need to convert from microseconds to seconds: duration_sec =
|
|
// duration_us / 1000000;
|
|
// * Now we calculate the distance based on the speed of sound (340 m/s):
|
|
// distance = duration_sec * 340;
|
|
// * The result calculation is therefore:
|
|
this->distance = (float)duration_us / 2 / 1000000 * 340;
|
|
|
|
// Serial.println(this->distance);
|
|
|
|
// std::cout << "US distance " << this->distance << std::endl;
|
|
|
|
// Filter faulty measurements. The sensor can often give values > 30 m which
|
|
// are not correct
|
|
// if (distance > 30)
|
|
// distance = 0;
|
|
|
|
return this->distance;
|
|
}
|
|
|
|
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
|
|
GetDistance();
|
|
Thing::Update(currentTimeMs, recursive);
|
|
}
|
|
|
|
#pragma region Touch sensor
|
|
|
|
UltrasonicSensor::TouchSensor::TouchSensor(
|
|
UltrasonicSensor::Configuration config,
|
|
Thing& parent)
|
|
: RoboidControl::TouchSensor(&parent), ultrasonic(config, &parent) {
|
|
this->touchedSomething = false;
|
|
}
|
|
UltrasonicSensor::TouchSensor::TouchSensor(
|
|
UltrasonicSensor::Configuration config,
|
|
Thing* parent)
|
|
: RoboidControl::TouchSensor(parent), ultrasonic(config, parent) {
|
|
this->touchedSomething = false;
|
|
}
|
|
|
|
void UltrasonicSensor::TouchSensor::Update(unsigned long currentTimeMs,
|
|
bool recursive) {
|
|
this->ultrasonic.Update(currentTimeMs, recursive);
|
|
this->touchedSomething = (this->ultrasonic.distance > 0 &&
|
|
this->ultrasonic.distance <= this->touchDistance);
|
|
}
|
|
|
|
#pragma region Touch sensor
|
|
|
|
} // namespace Arduino
|
|
} // namespace RoboidControl
|