Add tracked object type

Perception.cpp

@@ -6,14 +6,14 @@
 
 #include <math.h>
 
-// #define RC_DEBUG2
+#define RC_DEBUG2
 #ifdef RC_DEBUG2
 #include <Arduino.h>
 #endif
 
-unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
-                                              // number of object which can
-                                              // be tracked by a human
+unsigned char Perception::maxObjectCount = 70; // 7 is typically the maximum
+                                               // number of object which can
+                                               // be tracked by a human
 
 Perception::Perception() {
   this->trackedObjects = new InterestingThing *[maxObjectCount];
@@ -124,8 +124,10 @@ bool Perception::ObjectNearby(float direction, float range) {
   return false;
 }
 
-void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
+void Perception::AddTrackedObject(Sensor *sensor, Polar position,
+                                  unsigned char objectType) {
   InterestingThing *obj = new InterestingThing(sensor, position);
+  obj->type = objectType;
 
   unsigned char farthestObjIx = 0;
   unsigned char availableSlotIx = 0;
@@ -142,6 +144,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
 #endif
 
         this->trackedObjects[objIx]->Refresh(obj->position);
+        this->trackedObjects[objIx]->type = objectType;
         delete obj;
         return;
       }
@@ -164,7 +167,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
     Serial.print((int)obj->id);
     Serial.println(": new tracked object");
 #endif
-
+    roboid->networkSync->NewObject(obj);
   }
   // If this object is closer than the farthest object, then replace it
   else if (obj->position.distance <
@@ -331,14 +334,14 @@ void Perception::Update(float currentTimeMs) {
       if (distance >= 0) {
         float angle = sensor->position.angle;
         Polar position = Polar(angle, distance);
-        AddTrackedObject(distanceSensor, position);
+        // AddTrackedObject(distanceSensor, position);
       }
 
     } else if (sensor->type == Thing::SwitchType) {
       Switch *switchSensor = (Switch *)sensor;
       if (switchSensor->IsOn()) {
         Polar position = Polar(sensor->position.angle, nearbyDistance);
-        AddTrackedObject(switchSensor, position);
+        // AddTrackedObject(switchSensor, position);
       }
     } else {
       sensor->Update();

Perception.h

@@ -85,7 +85,8 @@ public:
   /// @param sensor The sensor which has detected the object
   /// @param position The position of the sensor in polar coordinates local to
   /// the roboid
-  void AddTrackedObject(Sensor *sensor, Polar position);
+  void AddTrackedObject(Sensor *sensor, Polar position,
+                        unsigned char objectType = 0x00);
   InterestingThing *
   AddTrackedObject(Sensor *sensor, Spherical position,
                    Quaternion orientation = Quaternion::identity);
@@ -135,7 +136,7 @@ public:
   /// nearby.
   /// @details This value is used by the ObjectNearby function to select the
   /// objects
-  float nearbyDistance = 0.3F;
+  float nearbyDistance = 0.02F;
 
 public:
   /// @brief The Sensors used for Perception

TrackedObject.h

@@ -74,7 +74,7 @@ public:
 
 protected:
   static constexpr unsigned char maxConfidence = 255;
-  static constexpr unsigned char confidenceDropSpeed = 150; // 2;
+  static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2;
 };
 
 } // namespace RoboidControl