diff --git a/DistanceSensor.cpp b/DistanceSensor.cpp index 5502abe..676a709 100644 --- a/DistanceSensor.cpp +++ b/DistanceSensor.cpp @@ -1,16 +1,12 @@ #include "DistanceSensor.h" -DistanceSensor::DistanceSensor() { - this->type = Thing::DistanceSensorType; -} +DistanceSensor::DistanceSensor() { this->type = Thing::DistanceSensorType; } DistanceSensor::DistanceSensor(float triggerDistance) { this->triggerDistance = triggerDistance; } -float DistanceSensor::GetDistance() { - return distance; -}; +float DistanceSensor::GetDistance() { return distance; }; bool DistanceSensor::ObjectNearby() { bool isOn = GetDistance() <= triggerDistance; diff --git a/Perception.cpp b/Perception.cpp index 008f219..9a33ad2 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -84,25 +84,25 @@ bool Perception::ObjectNearby(float direction, float range) { if (range < 0) range = -range; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - if (placement.horizontalDirection > direction - range && - placement.horizontalDirection < direction + range) { - Thing *thing = placement.thing; - if (thing == nullptr) - continue; + // for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + // Placement placement = sensorPlacements[sensorIx]; + // if (placement.horizontalDirection > direction - range && + // placement.horizontalDirection < direction + range) { + // Thing *thing = placement.thing; + // if (thing == nullptr) + // continue; - if (thing->type == Thing::DistanceSensorType) { - DistanceSensor *distanceSensor = (DistanceSensor *)thing; - if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) - return true; - } else if (thing->type == Thing::SwitchType) { - Switch *switchSensor = (Switch *)thing; - if (switchSensor != nullptr && switchSensor->IsOn()) - return true; - } - } - } + // if (thing->type == Thing::DistanceSensorType) { + // DistanceSensor *distanceSensor = (DistanceSensor *)thing; + // if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) + // return true; + // } else if (thing->type == Thing::SwitchType) { + // Switch *switchSensor = (Switch *)thing; + // if (switchSensor != nullptr && switchSensor->IsOn()) + // return true; + // } + // } + // } for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { PerceivedObject *obj = perceivedObjects[objIx]; @@ -243,6 +243,32 @@ void Perception::Update(float currentTimeMs) { lastUpdateTimeMs = currentTimeMs; + // Update sensing + for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { + Placement thingPlacement = sensorPlacements[sensorIx]; + Thing *thing = thingPlacement.thing; + if (thing == nullptr) + continue; + + if (thing->type == Thing::DistanceSensorType) { + DistanceSensor *distanceSensor = (DistanceSensor *)thing; + printf("S%d: %d %f\n", sensorIx, (int)distanceSensor, + distanceSensor->GetDistance()); + if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) { + Polar position = Polar(thingPlacement.horizontalDirection, + distanceSensor->GetDistance()); + AddPerceivedObject(position); + } + } else if (thing->type == Thing::SwitchType) { + Switch *switchSensor = (Switch *)thing; + if (switchSensor != nullptr && switchSensor->IsOn()) { + Polar position = + Polar(thingPlacement.horizontalDirection, nearbyDistance); + AddPerceivedObject(position); + } + } + } + for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { PerceivedObject *obj = perceivedObjects[objIx]; if (obj == nullptr)