Separated out perceived object

CMakeLists.txt

@@ -43,6 +43,7 @@ include_directories(
 add_library(RoboidControl STATIC 
     "Roboid.cpp"
     "Perception.cpp"
+    "PerceivedObject.cpp"
     "Propulsion.cpp"
     "Motor.cpp"
     "DifferentialDrive.cpp"

DifferentialDrive.cpp

@@ -55,8 +55,8 @@ Polar DifferentialDrive::GetVelocity() {
   float leftSpeed = leftMotor->GetSpeed();
   float rightSpeed = rightMotor->GetSpeed();
   float speed = (leftSpeed + rightSpeed) / 2;
-  float direction = speed >= 0 ? 0 : 180;
-  float distance = abs(speed);
+  float direction = speed >= 0 ? 0.0F : 180.0F;
+  float distance = fabsf(speed);
   Polar velocity = Polar(direction, distance);
   return velocity;
 }

PerceivedObject.cpp

@@ -0,0 +1,54 @@
+#include "PerceivedObject.h"
+
+#include <math.h>
+
+/***
+ * Oject perception
+ ***/
+
+PerceivedObject::PerceivedObject() {
+  this->id = 0;
+  this->confidence = maxConfidence;
+  this->position = Polar(0, INFINITY);
+  this->radius = INFINITY;
+}
+
+PerceivedObject::PerceivedObject(Sensor *sensor, Polar position, float radius)
+    : PerceivedObject() {
+  this->sensor = sensor;
+  this->position = position;
+  this->radius = radius;
+}
+
+bool PerceivedObject::IsTheSameAs(PerceivedObject *otherObj) {
+  if (id != 0 && id == otherObj->id)
+    return true;
+  if (fabsf(position.distance - otherObj->position.distance) > equalityDistance)
+    return false;
+  if (fabsf(position.angle - otherObj->position.angle) > equalityAngle)
+    return false;
+  return true;
+}
+
+bool PerceivedObject::DegradeConfidence(float deltaTime) {
+  unsigned char confidenceDrop =
+      (unsigned char)((float)confidenceDropSpeed * deltaTime);
+  // Make sure the confidence always drops
+  if (confidenceDrop == 0)
+    confidenceDrop = 1;
+
+  if (confidence <= confidenceDrop) {
+    // object is dead
+    confidence = 0;
+    return false;
+  } else {
+    confidence -= confidenceDrop;
+    return true;
+  }
+}
+
+void PerceivedObject::Refresh(Polar position, float radius) {
+  this->position = position;
+  this->radius = radius;
+  this->confidence = maxConfidence;
+}

PerceivedObject.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "Polar.h"
+#include "Sensor.h"
+
+class PerceivedObject {
+public:
+  PerceivedObject();
+  PerceivedObject(Sensor *sensor, Polar position, float radius = 0.1F);
+
+  static constexpr float equalityDistance = 0.3F;
+  static constexpr float equalityAngle = 5.0F;
+  bool IsTheSameAs(PerceivedObject *otherObj);
+
+  char id;
+
+  Polar position = Polar::zero;
+  float radius;
+  Sensor *sensor = nullptr;
+
+  static constexpr char maxConfidence = 255;
+  static constexpr char confidenceDropSpeed = 2;
+  char confidence;
+  bool DegradeConfidence(float deltaTime);
+  void Refresh(Polar position, float radius);
+};

Perception.cpp

@@ -95,56 +95,6 @@ bool Perception::ObjectNearby(float direction, float range) {
   return false;
 }
 
-/***
- * Oject perception
- ***/
-
-PerceivedObject::PerceivedObject() {
-  this->id = 0;
-  this->confidence = maxConfidence;
-  this->position = Polar(0, INFINITY);
-  this->radius = INFINITY;
-}
-
-PerceivedObject::PerceivedObject(Sensor *sensor, Polar position, float radius)
-    : PerceivedObject() {
-  this->sensor = sensor;
-  this->position = position;
-  this->radius = radius;
-}
-
-bool PerceivedObject::IsTheSameAs(PerceivedObject *otherObj) {
-  if (id != 0 && id == otherObj->id)
-    return true;
-  if (abs(position.distance - otherObj->position.distance) > equalityDistance)
-    return false;
-  if (abs(position.angle - otherObj->position.angle) > equalityAngle)
-    return false;
-  return true;
-}
-
-bool PerceivedObject::DegradeConfidence(float deltaTime) {
-  unsigned char confidenceDrop =
-      (unsigned char)((float)confidenceDropSpeed * deltaTime);
-  // Make sure the confidence always drops
-  if (confidenceDrop == 0)
-    confidenceDrop = 1;
-
-  if (confidence <= confidenceDrop) {
-    // object is dead
-    confidence = 0;
-    return false;
-  } else {
-    confidence -= confidenceDrop;
-    return true;
-  }
-}
-
-void PerceivedObject::Refresh(Polar position, float radius) {
-  this->position = position;
-  this->radius = radius;
-  this->confidence = maxConfidence;
-}
 
 void Perception::AddPerceivedObject(Sensor *sensor, Polar position) {
   // int objCount = PerceivedObjectCount();

Perception.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "PerceivedObject.h"
 #include "Placement.h"
 #include "Polar.h"
 #include "Quaternion.h"
@@ -10,28 +11,6 @@ namespace RoboidControl {
 
 class Roboid;
 
-class PerceivedObject {
-public:
-  PerceivedObject();
-  PerceivedObject(Sensor *sensor, Polar position, float radius = 0.1F);
-
-  static constexpr float equalityDistance = 0.3F;
-  static constexpr float equalityAngle = 5.0F;
-  bool IsTheSameAs(PerceivedObject *otherObj);
-
-  char id;
-
-  Polar position = Polar::zero;
-  float radius;
-  Sensor *sensor = nullptr;
-
-  static constexpr char maxConfidence = 255;
-  static constexpr char confidenceDropSpeed = 2;
-  char confidence;
-  bool DegradeConfidence(float deltaTime);
-  void Refresh(Polar position, float radius);
-};
-
 /// @brief Module to which keeps track of objects around the roboid
 class Perception {
 public: