38 lines
971 B
C++
38 lines
971 B
C++
#include "DistanceSensor.h"
|
|
|
|
#include "ControlCore/LowLevelMessages.h"
|
|
#include <math.h>
|
|
|
|
DistanceSensor::DistanceSensor() : Sensor() {
|
|
this->type = (unsigned char)Type::DistanceSensor;
|
|
this->distance = INFINITY;
|
|
this->triggerDistance = 1.0F;
|
|
}
|
|
|
|
DistanceSensor::DistanceSensor(float triggerDistance) : DistanceSensor() {
|
|
this->triggerDistance = triggerDistance;
|
|
}
|
|
|
|
float DistanceSensor::GetDistance() {
|
|
if (this->distance < minRange || this->distance > maxRange)
|
|
return INFINITY; // invalid distance
|
|
|
|
float d = this->distance;
|
|
this->distance = INFINITY;
|
|
return d;
|
|
};
|
|
|
|
bool DistanceSensor::ObjectNearby() {
|
|
if (distance < minRange || distance > maxRange)
|
|
return false;
|
|
|
|
bool isOn = distance <= triggerDistance;
|
|
return isOn;
|
|
}
|
|
|
|
void DistanceSensor::ProcessBytes(unsigned char *bytes) {
|
|
unsigned char ix = 0;
|
|
this->distance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
|
|
// std::cout << "received distance " << this->distance << "\n";
|
|
}
|