Improved object perception

Perception.cpp

@@ -156,23 +156,35 @@ void PerceivedObject::Refresh(Polar position, float radius) {
   this->confidence = maxConfidence;
 }
 
+void Perception::AddPerceivedObject(Polar position) {
+  // int objCount = PerceivedObjectCount();
+  // printf("perc obj count %d\n");
+
+  PerceivedObject *obj = new PerceivedObject(position);
+  // objCount = PerceivedObjectCount();
+  // printf("perc obj count %d\n");
+  AddPerceivedObject(obj);
+}
+
 void Perception::AddPerceivedObject(PerceivedObject *obj) {
   unsigned char farthestObjIx = 0;
   unsigned char availableSlotIx = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    // printf("[%d] %d\n", objIx, this->perceivedObjects[objIx]);
     // Is this slot available?
-    if (perceivedObjects[objIx] == nullptr) {
+    if (this->perceivedObjects[objIx] == nullptr) {
       availableSlotIx = objIx;
     }
     // Do we see the same object?
-    else if (obj->IsTheSameAs(perceivedObjects[objIx])) {
-      perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
+    else if (obj->IsTheSameAs(this->perceivedObjects[objIx])) {
+      printf("[%d] Updating...\n", objIx);
+      this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
       return;
     }
     // Is this the fartest object we see?
-    else if (perceivedObjects[farthestObjIx] == nullptr ||
-             (perceivedObjects[objIx]->position.distance >
-              perceivedObjects[farthestObjIx]->position.distance)) {
+    else if (this->perceivedObjects[farthestObjIx] == nullptr ||
+             (this->perceivedObjects[objIx]->position.distance >
+              this->perceivedObjects[farthestObjIx]->position.distance)) {
       farthestObjIx = objIx;
     }
   }
@@ -181,12 +193,13 @@ void Perception::AddPerceivedObject(PerceivedObject *obj) {
   // max number of objects)
   if (availableSlotIx < maxObjectCount) {
     // a slot is available
-    perceivedObjects[availableSlotIx] = obj;
+    printf("[%d] new object \n", availableSlotIx);
+    this->perceivedObjects[availableSlotIx] = obj;
   }
   // If this object is closer than the farthest object, then replace it
   else if (obj->position.distance <
-           perceivedObjects[farthestObjIx]->position.distance) {
-    perceivedObjects[farthestObjIx] = obj;
+           this->perceivedObjects[farthestObjIx]->position.distance) {
+    this->perceivedObjects[farthestObjIx] = obj;
     // we may want to destroy the fartest object, but if it is created
     // externally, other links may still exist...
   }
@@ -195,13 +208,15 @@ void Perception::AddPerceivedObject(PerceivedObject *obj) {
 unsigned char Perception::PerceivedObjectCount() {
   unsigned char objectCount = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    if (perceivedObjects[objIx] != nullptr)
+    if (this->perceivedObjects[objIx] != nullptr)
       objectCount++;
   }
   return objectCount;
 }
 
-PerceivedObject **Perception::GetPerceivedObjects() { return perceivedObjects; }
+PerceivedObject **Perception::GetPerceivedObjects() {
+  return this->perceivedObjects;
+}
 
 void Perception::Update(float currentTimeMs) {
   float deltaTime = currentTimeMs - lastUpdateTimeMs;
@@ -217,12 +232,13 @@ void Perception::Update(float currentTimeMs) {
 
     if (obj->DegradeConfidence(deltaTime) == false) {
       // delete obj
-      perceivedObjects[objIx] = nullptr;
+      printf("[%d] delete object\n", objIx);
+      this->perceivedObjects[objIx] = nullptr;
     } else {
-      // Serial.printf("[%d] confidence: %d\n", objIx,
-      //               perceivedObjects[objIx]->confidence);
+      Serial.printf("[%d] confidence: %d\n", objIx,
+                    this->perceivedObjects[objIx]->confidence);
     }
   }
-  if (perceivedObjects[0] != nullptr) {
+  if (this->perceivedObjects[0] != nullptr) {
   }
 }

Perception.h

@@ -10,7 +10,7 @@ namespace RoboidControl {
 class PerceivedObject {
 public:
   PerceivedObject();
-  PerceivedObject(Polar position, float radius);
+  PerceivedObject(Polar position, float radius = 0.1F);
 
   static constexpr float equalityDistance = 0.3F;
   static constexpr float equalityAngle = 5.0F;
@@ -94,6 +94,7 @@ public:
 
   // Object Perception
 
+  void AddPerceivedObject(Polar position);
   void AddPerceivedObject(PerceivedObject *obj);
   unsigned char PerceivedObjectCount();
   PerceivedObject **GetPerceivedObjects();

Propulsion.cpp

@@ -7,9 +7,7 @@ Propulsion::Propulsion() {
   this->motorCount = 0;
 }
 
-unsigned int Propulsion::GetMotorCount() {
-  return this->motorCount;
-}
+unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
 
 Motor *Propulsion::GetMotor(unsigned int motorId) {
   if (motorId >= this->motorCount)
@@ -33,14 +31,13 @@ Placement* Propulsion::GetMotorPlacement(unsigned int motorId) {
   return nullptr;
 }
 
-
 void Propulsion::Update(float currentTimeMs) {}
 
 void Propulsion::SetTwistSpeed(float forward, float yaw) {}
 
 void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
 
-void Propulsion::SetTwistSpeed(Vector3 linear,
-                               float yaw,
-                               float pitch,
+void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
                                float roll) {}
+
+Polar Propulsion::GetVelocity() { return Polar(0, 0); }

Propulsion.h

@@ -2,6 +2,7 @@
 
 #include "Motor.h"
 #include "Placement.h"
+#include "Polar.h"
 #include "Vector2.h"
 
 namespace Passer {
@@ -56,10 +57,10 @@ class Propulsion {
   /// @param yaw The target rotation speed around the vertical axis
   /// @param pitch The target rotation speed around the sideward axis
   /// @param roll The target rotation speed around hte forward axis
-  virtual void SetTwistSpeed(Vector3 linear,
-                             float yaw = 0.0F,
-                             float pitch = 0.0F,
-                             float roll = 0.0F);
+  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
+                             float pitch = 0.0F, float roll = 0.0F);
+
+  virtual Polar GetVelocity();
 
 protected:
   /// @brief The number of motors used for Propulsion