replace sensor scan by object scan

Perception.cpp

@@ -1,6 +1,7 @@
 #include "Perception.h"
 #include "Angle.h"
 #include "DistanceSensor.h"
+#include "RoboidWiFi.h"
 #include "Switch.h"
 
 #include <Arduino.h>
@@ -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++) {
+  // for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
-    Placement placement = sensorPlacements[sensorIx];
+  //   Placement placement = sensorPlacements[sensorIx];
-    // This still needs support for angles wrapping around 180 degrees !!!!
+  //   // This still needs support for angles wrapping around 180 degrees !!!!
-    if (placement.horizontalDirection > direction - range &&
+  //   if (placement.horizontalDirection > direction - range &&
-        placement.horizontalDirection < direction + range) {
+  //       placement.horizontalDirection < direction + range) {
-      Thing *thing = placement.thing;
+  //     Thing *thing = placement.thing;
-      if (thing == nullptr)
+  //     if (thing == nullptr)
-        continue;
+  //       continue;
 
-      if (thing->type == Thing::DistanceSensorType) {
+  //     if (thing->type == Thing::DistanceSensorType) {
-        DistanceSensor *distanceSensor = (DistanceSensor *)thing;
+  //       DistanceSensor *distanceSensor = (DistanceSensor *)thing;
-        if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
+  //       if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
-          minDistance = fmin(minDistance, distanceSensor->GetDistance());
+  //         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 &&
+  //   // This still needs support for angles wrapping around 180 degrees !!!!
-  //       placement.horizontalDirection < direction + range) {
+  //   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;
+  //         minDistance = fmin(minDistance, distanceSensor->GetDistance());
-  //     } 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];
+    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,
+      float distance = distanceSensor->GetDistance();
-             distanceSensor->GetDistance());
+      float angle = thingPlacement.horizontalDirection;
-             
+      Polar position = Polar(angle, distance);
-      if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) {
-        Polar position = Polar(thingPlacement.horizontalDirection,
-                               distanceSensor->GetDistance());
       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 {
+      // } else {
-      Serial.printf("[%d] confidence: %d\n", objIx,
+      //   Serial.printf("[%d] confidence: %d\n", objIx,
-                    this->perceivedObjects[objIx]->confidence);
+      //                 this->perceivedObjects[objIx]->confidence);
     }
   }
   if (this->perceivedObjects[0] != nullptr) {

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();