19 lines
553 B
C++
19 lines
553 B
C++
#include "Perception.h"
|
|
#include <Arduino.h>
|
|
#include "Angle.h"
|
|
|
|
DistanceSensor* Perception::GetSensor(float angle) {
|
|
angle = Angle::Normalize(angle);
|
|
|
|
for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
|
|
Placement placement = this->sensorPlacements[ix];
|
|
if (abs(placement.direction.y - angle) < 0.01F)
|
|
return (DistanceSensor*)placement.thing;
|
|
}
|
|
|
|
DistanceSensor* distanceSensor = new DistanceSensor();
|
|
Placement* newPlacement = new Placement(distanceSensor, angle);
|
|
AddSensors(newPlacement, 1);
|
|
|
|
return distanceSensor;
|
|
} |