20 lines
604 B
C++
20 lines
604 B
C++
#include "Perception.h"
|
|
#include "Angle.h"
|
|
#include <Arduino.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();
|
|
Serial.printf("New sensor ADDED %f \n", angle);
|
|
Placement *newPlacement = new Placement(distanceSensor, angle);
|
|
AddSensors(newPlacement, 1);
|
|
|
|
return distanceSensor;
|
|
} |