115 lines
3.6 KiB
C++
115 lines
3.6 KiB
C++
#include <DistanceSensor.h>
|
|
#include <Sensing.h>
|
|
#include <Switch.h>
|
|
|
|
#include <math.h>
|
|
#include <algorithm>
|
|
|
|
SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor, Vector2 direction) {
|
|
this->distanceSensor = distanceSensor;
|
|
this->switchSensor = nullptr;
|
|
this->direction = direction;
|
|
}
|
|
SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) {
|
|
this->distanceSensor = nullptr;
|
|
this->switchSensor = switchSensor;
|
|
this->direction = direction;
|
|
}
|
|
|
|
Sensing::Sensing() {}
|
|
|
|
// void Sensing::AddSensors(SensorPlacement* sensors, unsigned int sensorCount) {
|
|
// this->sensors = sensors;
|
|
// this->sensorCount = sensorCount;
|
|
// }
|
|
|
|
void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
|
|
sensorCount = 0;
|
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
|
Thing* thing = things[thingIx].thing;
|
|
if (thing->isSensor)
|
|
sensorCount++;
|
|
}
|
|
|
|
sensorPlacements = new Placement[sensorCount];
|
|
|
|
unsigned int sensorIx = 0;
|
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
|
Thing* thing = things[thingIx].thing;
|
|
if (thing->isSensor)
|
|
sensorPlacements[sensorIx++] = things[thingIx];
|
|
}
|
|
}
|
|
|
|
float Sensing::DistanceForward(float angle) {
|
|
float minDistance = INFINITY;
|
|
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
|
Placement placement = sensorPlacements[sensorIx];
|
|
Sensor* sensor = (Sensor*)placement.thing;
|
|
if (sensor->isDistanceSensor == false)
|
|
continue;
|
|
|
|
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
|
|
float sensorAngle = placement.direction.x;
|
|
if (sensorAngle > -angle && sensorAngle < angle) {
|
|
minDistance = fmin(minDistance, distanceSensor->GetDistance());
|
|
}
|
|
}
|
|
return minDistance;
|
|
}
|
|
|
|
float Sensing::DistanceLeft(float angle) {
|
|
float minDistance = INFINITY;
|
|
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
|
Placement placement = sensorPlacements[sensorIx];
|
|
Sensor* sensor = (Sensor*)placement.thing;
|
|
if (sensor->isDistanceSensor == false)
|
|
continue;
|
|
|
|
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
|
|
float sensorAngle = placement.direction.x;
|
|
// Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle);
|
|
if (sensorAngle < 0 && sensorAngle > -angle) {
|
|
minDistance = fmin(minDistance, distanceSensor->GetDistance());
|
|
}
|
|
}
|
|
return minDistance;
|
|
}
|
|
|
|
float Sensing::DistanceRight(float angle) {
|
|
float minDistance = INFINITY;
|
|
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
|
Placement placement = sensorPlacements[sensorIx];
|
|
Sensor* sensor = (Sensor*)placement.thing;
|
|
if (sensor->isDistanceSensor == false)
|
|
continue;
|
|
|
|
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
|
|
float sensorAngle = placement.direction.x;
|
|
// Serial.printf(" distance sensor: 0 %f %f\n", sensorAngle, angle);
|
|
if (sensorAngle > 0 && sensorAngle < angle) {
|
|
minDistance = fmin(minDistance, distanceSensor->GetDistance());
|
|
}
|
|
}
|
|
return minDistance;
|
|
}
|
|
|
|
bool Sensing::SwitchOn(float fromAngle, float toAngle) {
|
|
if (toAngle < fromAngle)
|
|
return false;
|
|
|
|
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
|
Placement placement = sensorPlacements[sensorIx];
|
|
float angle = placement.direction.x;
|
|
if (angle > fromAngle && angle < toAngle) {
|
|
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
|
|
|
|
// if (placement.switchSensor != nullptr && placement.switchSensor->IsOn())
|
|
// return true;
|
|
// else
|
|
if (distanceSensor != nullptr && distanceSensor->IsOn())
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
} |