diff --git a/Perception.cpp b/Perception.cpp index be8a886..d2b690b 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -6,14 +6,14 @@ #include -// #define RC_DEBUG2 +#define RC_DEBUG2 #ifdef RC_DEBUG2 #include #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(); diff --git a/Perception.h b/Perception.h index 730624f..67bf2cc 100644 --- a/Perception.h +++ b/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 diff --git a/TrackedObject.h b/TrackedObject.h index 6d15a93..3c50755 100644 --- a/TrackedObject.h +++ b/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