Use thingId from ControlCore

ControlCore/CoreThing.cpp

@@ -1,14 +1,18 @@
 #include "CoreThing.h"
 
-CoreThing::CoreThing(
-    // Participant *client,
-    unsigned char networkId, unsigned char thingId, unsigned char thingType) {
-  // this->client = client;
-  this->id = thingId;
+#include <iostream>
+
+CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
   this->type = thingType;
   this->networkId = networkId;
   this->Init();
-  CoreThing::Add(this);
+
+  int thingId = CoreThing::Add(this);
+  if (thingId < 0) {
+    std::cout << "ERROR: Thing store is full\n";
+    this->id = 0; // what to do when we cannot store any more things?
+  } else
+    this->id = thingId;
 }
 
 void CoreThing::Init() {}
@@ -26,13 +30,14 @@ CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
   return nullptr;
 }
 
-bool CoreThing::Add(CoreThing *newThing) {
+int CoreThing::Add(CoreThing *newThing) {
   for (unsigned char ix = 0; ix < 256; ix++) {
     CoreThing *thing = allThings[ix];
     if (thing == nullptr) {
       allThings[ix] = newThing;
-      return true;
+
+      return ix;
     }
   }
-  return false;
+  return -1;
 }

ControlCore/CoreThing.h

@@ -6,6 +6,7 @@ class CoreThing {
 public:
   // Participant *client;
   unsigned char networkId;
+  /// @char The id of the thing
   unsigned char id;
   // CoreThing *parent;
   /// @brief The type of Thing
@@ -14,16 +15,32 @@ public:
   const char *modelUrl = nullptr;
   // protected Sensor sensor;
 
+  /// @brief Basic Thing types
+  enum class Type {
+    Undetermined,
+    // Sensor,
+    Switch,
+    DistanceSensor,
+    DirectionalSensor,
+    TemperatureSensor,
+    // Motor,
+    ControlledMotor,
+    UncontrolledMotor,
+    Servo,
+    // Other
+    Humanoid,
+    ExternalSensor,
+  };
+
+private:
   static CoreThing *allThings[];
 
 public:
-  CoreThing(
-      // Participant *client,
-      unsigned char networkId, unsigned char thingId,
-      unsigned char thingType = 0);
+  CoreThing(unsigned char networkId = 0,
+            unsigned char thingType = (unsigned char)Type::Undetermined);
 
   static CoreThing *Get(unsigned char networkId, unsigned char thingId);
-  static bool Add(CoreThing *thing);
+  static int Add(CoreThing *thing);
 
 protected:
   virtual void Init();

Motor.cpp

@@ -2,9 +2,7 @@
 
 #include <time.h>
 
-Motor::Motor() : Thing(0) { // for now, id should be set properly later
-  this->type = (int)Thing::UncontrolledMotorType;
-}
+Motor::Motor() : Thing() { this->type = (int)Thing::UncontrolledMotorType; }
 
 float Motor::GetActualSpeed() { return this->currentTargetSpeed; }
 

NetworkSync.cpp

@@ -1,6 +1,6 @@
 #include "NetworkSync.h"
 
-#define RC_DEBUG 1
+// #define RC_DEBUG 1
 
 #ifdef RC_DEBUG
 #include <Arduino.h>

Roboid.cpp

@@ -11,7 +11,7 @@
 #include <Arduino.h>
 #endif
 
-Roboid::Roboid() : Thing(0) {
+Roboid::Roboid() : Thing() {
 #ifdef RC_DEBUG
   Serial.begin(115200);
 #endif
@@ -66,40 +66,6 @@ void Roboid::Update(unsigned long currentTimeMs) {
   lastUpdateTimeMs = currentTimeMs;
 }
 
-/*
-void Roboid::SetPosition(Spherical16 newWorldPosition) {
-  SwingTwist16 roboidOrientation = this->GetOrientation();
-  Spherical16 translation = newWorldPosition - this->roboidPosition;
-  float distance = translation.distance;
-  Angle16 angle = Spherical16::SignedAngleBetween(
-      roboidOrientation * Spherical16::forward, translation, Spherical16::up);
-  Polar16 polarTranslation = Polar16(
-      distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
-  if (perception != nullptr) {
-    printf("roboid translation %f, %f\n", polarTranslation.distance,
-           polarTranslation.angle.InDegrees());
-    perception->UpdatePose(polarTranslation);
-  }
-
-  this->position = newWorldPosition; // roboid is the root?
-  // World position should be set in the update recursion
-  // this->worldPosition = newWorldPosition;
-
-  // if (networkSync != nullptr)
-  //   // networkSync->SendPosition(this->worldPosition);
-  //   networkSync->SendPose(this->worldPosition, roboidOrientation);
-}
-
-#include <math.h>
-void Roboid::SetOrientation(SwingTwist16 newOrientation) {
-  SwingTwist16 delta =
-      SwingTwist16::Inverse(this->orientation) * newOrientation;
-  if (perception != nullptr)
-    perception->UpdatePose(delta);
-
-  this->orientation = newOrientation;
-}
-*/
 void Roboid::AddChild(Thing *child) {
   Thing::AddChild(child);
   if (child->IsSensor()) {
@@ -112,6 +78,7 @@ void Passer::RoboidControl::Roboid::Release(Thing *child) {
   if (RemoveChild(child) != nullptr) {
     child->position = this->position;
     child->orientation = this->orientation;
+    // this creates an new thing, I wish I could avoid this.
     this->perception->AddTrackedObject(nullptr, child);
   }
 }

ServoMotor.cpp

@@ -2,8 +2,7 @@
 
 #include "LinearAlgebra/FloatSingle.h"
 
-ServoMotor::ServoMotor()
-    : Thing(0) {  // for now, id should be set properly later
+ServoMotor::ServoMotor() : Thing() {
   this->type = Thing::ServoType;
   this->controlMode = ControlMode::Position;
   this->targetAngle = Angle16();
@@ -23,9 +22,7 @@ void ServoMotor::SetTargetAngle(Angle16 angle) {
   this->hasTargetAngle = true;
 }
 
-Angle16 ServoMotor::GetTargetAngle() {
-  return this->targetAngle;
-}
+Angle16 ServoMotor::GetTargetAngle() { return this->targetAngle; }
 
 void ServoMotor::SetMaximumVelocity(float maxVelocity) {
   this->maxSpeed = maxVelocity;
@@ -36,18 +33,16 @@ void ServoMotor::SetTargetVelocity(float targetVelocity) {
 
   this->controlMode = ControlMode::Velocity;
   this->targetVelocity = targetVelocity;
-  this->hasTargetAngle = false;  // can't we use the controlMode for this?
+  this->hasTargetAngle = false; // can't we use the controlMode for this?
 }
 
-float ServoMotor::GetTargetVelocity() {
-  return this->targetVelocity;
-}
+float ServoMotor::GetTargetVelocity() { return this->targetVelocity; }
 
 void ServoMotor::Update(unsigned long currentTimeMs) {
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-    Thing* child = this->GetChild(childIx);
+    Thing *child = this->GetChild(childIx);
     if (child != nullptr && child->type == Thing::ServoType) {
-      ServoMotor* servo = (ServoMotor*)child;
+      ServoMotor *servo = (ServoMotor *)child;
       servo->Update(currentTimeMs);
     }
   }
@@ -100,6 +95,4 @@ void ServoMotor::Update(unsigned long currentTimeMs) {
   }
 }
 
-void ServoMotor::SetAngle(Angle16 angle) {
-  this->actualAngle = angle;
-};
+void ServoMotor::SetAngle(Angle16 angle) { this->actualAngle = angle; };

Thing.cpp

@@ -6,25 +6,14 @@ using namespace Passer::RoboidControl;
 
 int Thing::lastThingId = 1;
 
-Thing::Thing()
-    : CoreThing(0, lastThingId++, (unsigned char)Type::Undetermined) {
-  this->id = lastThingId++;
-  this->type = (unsigned int)Type::Undetermined;
+Thing::Thing() : CoreThing() {
+  // this->id = lastThingId++;
+  // this->type = (unsigned int)Type::Undetermined;
   this->childCount = 0;
   this->parent = nullptr;
   this->children = nullptr;
 }
 
-Thing::Thing(unsigned char id)
-    : CoreThing(0, id, (unsigned char)Type::Undetermined) {
-  this->id = id;
-  this->type = (unsigned int)Type::Undetermined;
-  this->childCount = 0;
-  this->parent = nullptr;
-  this->children = nullptr;
-  //  this->position = SphericalOf<signed short>::zero;
-}
-
 void Thing::SetName(const char *name) { this->name = name; }
 
 const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;

Thing.h

@@ -12,12 +12,10 @@ namespace RoboidControl {
 /// @brief A thing is a functional component on a robot
 class Thing : public CoreThing {
 public:
-  Thing();
   /// @brief Default constructor for a Thing
-  Thing(unsigned char id);
+  Thing();
 
-  /// @char The id of the thing
-  unsigned char id;
+  // unsigned char id;
 
   void SetName(const char *name);
 
@@ -120,21 +118,21 @@ protected:
   static const unsigned char CustomType = 0x80;
 
   /// @brief Basic Thing types
-  enum class Type {
-    Undetermined,
-    // Sensor,
-    Switch,
-    DistanceSensor,
-    DirectionalSensor,
-    TemperatureSensor,
-    // Motor,
-    ControlledMotor,
-    UncontrolledMotor,
-    Servo,
-    // Other
-    Humanoid,
-    ExternalSensor,
-  };
+  // enum class Type {
+  //   Undetermined,
+  //   // Sensor,
+  //   Switch,
+  //   DistanceSensor,
+  //   DirectionalSensor,
+  //   TemperatureSensor,
+  //   // Motor,
+  //   ControlledMotor,
+  //   UncontrolledMotor,
+  //   Servo,
+  //   // Other
+  //   Humanoid,
+  //   ExternalSensor,
+  // };
 
   Thing *parent = nullptr;
   Thing **children = nullptr;

TrackedObject.cpp

@@ -2,7 +2,7 @@
 
 #include <math.h>
 
-InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing(0) {
+InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing() {
   this->id = 0;
   this->confidence = maxConfidence;
   this->sensor = sensor;
@@ -14,7 +14,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing(0) {
 
 InterestingThing::InterestingThing(Sensor *sensor, Spherical16 position,
                                    SwingTwist16 orientation)
-    : Thing(0) {
+    : Thing() {
   this->id = 0;
   this->confidence = maxConfidence;
   this->sensor = sensor;