diff --git a/Perception.cpp b/Perception.cpp index 10aadab..999e64b 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -1,6 +1,7 @@ #include "Perception.h" #include "Angle.h" #include "DistanceSensor.h" +#include "RoboidWiFi.h" #include "Switch.h" #include @@ -45,20 +46,31 @@ float Perception::GetDistance(float direction, float range) { if (range < 0) range = -range; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - // This still needs support for angles wrapping around 180 degrees !!!! - 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]; + // // This still needs support for angles wrapping around 180 degrees !!!! + // 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()) - minDistance = fmin(minDistance, distanceSensor->GetDistance()); - } + // if (thing->type == Thing::DistanceSensorType) { + // DistanceSensor *distanceSensor = (DistanceSensor *)thing; + // if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) + // minDistance = fmin(minDistance, distanceSensor->GetDistance()); + // } + // } + // } + + for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { + PerceivedObject *obj = perceivedObjects[objIx]; + if (obj == nullptr) + continue; + if (obj->position.angle > direction - range && + obj->position.angle < direction + range) { + + minDistance = fmin(minDistance, obj->position.distance); } } return minDistance; @@ -70,34 +82,13 @@ float Perception::GetDistance(float horizontalDirection, if (range < 0) range = -range; - for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { - Placement placement = sensorPlacements[sensorIx]; - // This still needs support for angles wrapping around 180 degrees !!!! - if (placement.horizontalDirection > horizontalDirection - range && - placement.horizontalDirection < horizontalDirection + range && - placement.verticalDirection > verticalDirection - range && - placement.verticalDirection < verticalDirection + range) { - Thing *thing = placement.thing; - if (thing == nullptr) - continue; - - if (thing->type == Thing::DistanceSensorType) { - DistanceSensor *distanceSensor = (DistanceSensor *)thing; - if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) - minDistance = fmin(minDistance, distanceSensor->GetDistance()); - } - } - } - return minDistance; -} -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) { + // // This still needs support for angles wrapping around 180 degrees !!!! + // if (placement.horizontalDirection > horizontalDirection - range && + // placement.horizontalDirection < horizontalDirection + range && + // placement.verticalDirection > verticalDirection - range && + // placement.verticalDirection < verticalDirection + range) { // Thing *thing = placement.thing; // if (thing == nullptr) // continue; @@ -105,14 +96,26 @@ bool Perception::ObjectNearby(float direction, float range) { // 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; + // minDistance = fmin(minDistance, distanceSensor->GetDistance()); // } // } // } + for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { + PerceivedObject *obj = perceivedObjects[objIx]; + if (obj == nullptr) + continue; + if (obj->position.angle > horizontalDirection - range && + obj->position.angle < horizontalDirection + range) { + + minDistance = fmin(minDistance, obj->position.distance); + } + } + + return minDistance; +} +bool Perception::ObjectNearby(float direction, float range) { + if (range < 0) + range = -range; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { PerceivedObject *obj = perceivedObjects[objIx]; @@ -205,7 +208,7 @@ void Perception::AddPerceivedObject(Sensor *sensor, Polar position) { // this->perceivedObjects[objIx]->position.angle, // obj->position.distance, obj->position.angle); if (obj->IsTheSameAs(this->perceivedObjects[objIx])) { - printf("[%d] Updating...\n", objIx); + // printf("[%d] Updating...\n", objIx); this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius); return; } @@ -264,14 +267,10 @@ void Perception::Update(float currentTimeMs) { 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(distanceSensor, position); - } + float distance = distanceSensor->GetDistance(); + float angle = thingPlacement.horizontalDirection; + Polar position = Polar(angle, distance); + AddPerceivedObject(distanceSensor, position); } else if (thing->type == Thing::SwitchType) { Switch *switchSensor = (Switch *)thing; if (switchSensor != nullptr && switchSensor->IsOn()) { @@ -290,10 +289,12 @@ void Perception::Update(float currentTimeMs) { if (obj->DegradeConfidence(deltaTime) == false) { // delete obj printf("[%d] delete object\n", objIx); + if (roboid != nullptr && roboid->networkSync != nullptr) + roboid->networkSync->DestroyObject(obj); this->perceivedObjects[objIx] = nullptr; - } else { - Serial.printf("[%d] confidence: %d\n", objIx, - this->perceivedObjects[objIx]->confidence); + // } else { + // Serial.printf("[%d] confidence: %d\n", objIx, + // this->perceivedObjects[objIx]->confidence); } } if (this->perceivedObjects[0] != nullptr) { diff --git a/Perception.h b/Perception.h index 5e6f36b..4eabcad 100644 --- a/Perception.h +++ b/Perception.h @@ -8,6 +8,8 @@ namespace Passer { namespace RoboidControl { +class Roboid; + class PerceivedObject { public: PerceivedObject(); @@ -48,6 +50,8 @@ public: /// @param sensorCount The number of sensors in the placement array Perception(Placement *sensors, unsigned int sensorCount); + Roboid *roboid = nullptr; + /// @brief Get the number of Sensors /// @return The number of sensors, zero when no sensors are present unsigned int GetSensorCount();