AR mode works, but reaction time is slow

Arduino/ArduinoParticipant.cpp

@@ -82,19 +82,6 @@ void ParticipantUDP::Receive() {
     senderAddress.toCharArray(sender_ipAddress, 16);
     unsigned int sender_port = udp->remotePort();
 
-    // Participant* remoteParticipant = this->Get(sender_ipAddress,
-    // sender_port); if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->Add(sender_ipAddress,
-    //   sender_port);
-    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
-    //   //           << "\n";
-    //   // std::cout << "New remote participant " <<
-    //   remoteParticipant->ipAddress
-    //   //           << ":" << remoteParticipant->port << " "
-    //   //           << (int)remoteParticipant->networkId << "\n";
-    // }
-
-    // ReceiveData(packetSize, remoteParticipant);
     ReceiveData(packetSize, sender_ipAddress, sender_port);
     packetSize = udp->parsePacket();
   }
@@ -107,6 +94,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
   // << remoteParticipant->port << "\n";
 
   int n = 0;
+  int r = 0;
   do {
     if (n > 0) {
 #if !defined(NO_STD)
@@ -115,9 +103,13 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
       delay(10);
     }
     n++;
+
     udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
     udp->write((unsigned char*)buffer, bufferSize);
-  } while (udp->endPacket() == 0 && n < 10);
+    r = udp->endPacket();
+    // On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
+    // It is not cleary yet why
+  } while (r == 0 && n < 10);
 
 #endif
   return true;

Arduino/Things/DRV8833.cpp

@@ -7,14 +7,14 @@ namespace Arduino {
 
 #pragma region DRV8833
 
-DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) {
+DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
   this->pinStandby = config.standby;
   if (pinStandby != 255)
     pinMode(pinStandby, OUTPUT);
 
-  this->motorA = new DRV8833Motor(*this, config.AIn1, config.AIn2);
+  this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
   this->motorA->SetName("Motor A");
-  this->motorB = new DRV8833Motor(*this, config.BIn1, config.BIn2);
+  this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
   this->motorB->SetName("Motor B");
 }
 
@@ -23,11 +23,11 @@ DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) {
 #pragma region Differential drive
 
 DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
-                                              Thing& parent)
+                                              Thing* parent)
-    : RoboidControl::DifferentialDrive(parent), drv8833(config) {
+    : RoboidControl::DifferentialDrive(this->drv8833.motorA,
-    this->leftWheel = this->drv8833.motorA;
+                                       this->drv8833.motorB,
-    this->rightWheel = this->drv8833.motorB;
+                                       parent),
-}
+      drv8833(config, this) {}
 
 void DRV8833::DifferentialDrive::Update(bool recurse) {
   RoboidControl::DifferentialDrive::Update(recurse);
@@ -42,7 +42,7 @@ void DRV8833::DifferentialDrive::Update(bool recurse) {
 uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
 #endif
 
-DRV8833Motor::DRV8833Motor(DRV8833& driver,
+DRV8833Motor::DRV8833Motor(DRV8833* driver,
                            unsigned char pinIn1,
                            unsigned char pinIn2,
                            bool reverse)

Arduino/Things/DRV8833.h

@@ -18,12 +18,11 @@ class DRV8833 : public Thing {
     int AIn2;
     int BIn1;
     int BIn2;
-    int standby = 255;
+    int standby;
   };
 
   /// @brief Setup a DRV8833 motor controller
-  // DRV8833(Configuration config, Participant* owner = nullptr);
+  DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
-  DRV8833(Configuration config, Thing& parent = Thing::LocalRoot());
 
   DRV8833Motor* motorA = nullptr;
   DRV8833Motor* motorB = nullptr;
@@ -39,9 +38,7 @@ class DRV8833 : public Thing {
 
 class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
  public:
-  // DifferentialDrive(DRV8833::Configuration config,
+  DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
-  //                   Participant* participant = nullptr);
-  DifferentialDrive(DRV8833::Configuration config, Thing& parent = Thing::LocalRoot());
 
   virtual void Update(bool recurse = false) override;
 
@@ -60,7 +57,7 @@ class DRV8833Motor : public Motor {
   /// @param pinIn1 the pin number for the in1 signal
   /// @param pinIn2 the pin number for the in2 signal
   /// @param direction the forward turning direction of the motor
-  DRV8833Motor(DRV8833& driver,
+  DRV8833Motor(DRV8833* driver,
                unsigned char pinIn1,
                unsigned char pinIn2,
                bool reverse = false);

Arduino/Things/DigitalInput.cpp

@@ -7,7 +7,7 @@ namespace Arduino {
 
 #pragma region Digital input
 
-DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) {
+DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) {
   this->pin = pin;
   pinMode(this->pin, INPUT);
   std::cout << "digital input start\n";
@@ -23,7 +23,7 @@ void DigitalInput::Update(bool recursive) {
 
 #pragma region Touch sensor
 
-DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent)
+DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
     : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
 
 void DigitalInput::TouchSensor::Update(bool recursive) {
@@ -39,7 +39,7 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
 volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
 
 DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
-                                               Thing& parent)
+                                               Thing* parent)
     : RoboidControl::RelativeEncoder(parent),
       digitalInput(config.pin, parent),
       pulsesPerRevolution(config.pulsesPerRevolution) {}

Arduino/Things/DigitalInput.h

@@ -12,9 +12,9 @@ class DigitalInput : public Thing {
   /// @brief Create a new digital input
   /// @param participant The participant to use
   /// @param pin The digital pin
-  DigitalInput(Participant* participant, unsigned char pin);
+  //DigitalInput(Participant* participant, unsigned char pin);
   // DigitalInput(Thing* parent, unsigned char pin);
-  DigitalInput(unsigned char pin, Thing& parent);
+  DigitalInput(unsigned char pin, Thing* parent);
 
   bool isHigh = false;
   bool isLow = false;
@@ -35,7 +35,7 @@ class DigitalInput : public Thing {
 
 class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
  public:
-  TouchSensor(unsigned char pin, Thing& parent);
+  TouchSensor(unsigned char pin, Thing* parent);
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
   virtual void Update(bool recurse = false) override;
@@ -55,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
     unsigned char pulsesPerRevolution;
   };
 
-  RelativeEncoder(Configuration config, Thing& parent = Thing::LocalRoot());
+  RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
 
   unsigned char pulsesPerRevolution;
 

Arduino/Things/UltrasonicSensor.cpp

@@ -6,8 +6,9 @@
 namespace RoboidControl {
 namespace Arduino {
 
-UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent)
+UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
-    : Thing(parent) {
+    : Thing(Type::Undetermined, parent) {
+  this->name = "Ultrasonic sensor";
   this->pinTrigger = config.triggerPin;
   this->pinEcho = config.echoPin;
 
@@ -57,16 +58,14 @@ void UltrasonicSensor::Update(bool recursive) {
 
 #pragma region Touch sensor
 
-UltrasonicSensor::TouchSensor::TouchSensor(
+UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
-    UltrasonicSensor::Configuration config,
+    : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
-    Thing& parent)
-    : RoboidControl::TouchSensor(parent), ultrasonic(config) {}
 
 void UltrasonicSensor::TouchSensor::Update(bool recursive) {
-  this->ultrasonic.Update(false);
-  this->touchedSomething = (this->ultrasonic.distance > 0 &&
-                            this->ultrasonic.distance <= this->touchDistance);
   RoboidControl::TouchSensor::Update(recursive);
+  this->ultrasonic.Update(false);
+  this->touchedSomething |= (this->ultrasonic.distance > 0 &&
+                            this->ultrasonic.distance <= this->touchDistance);
 }
 
 #pragma region Touch sensor

Arduino/Things/UltrasonicSensor.h

@@ -13,7 +13,7 @@ class UltrasonicSensor : Thing {
     int echoPin;
   };
 
-  UltrasonicSensor(Configuration config, Thing& parent = Thing::LocalRoot());
+  UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
 
   // parameters
 
@@ -47,7 +47,7 @@ class UltrasonicSensor : Thing {
 class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
  public:
   TouchSensor(UltrasonicSensor::Configuration config,
-              Thing& parent = Thing::LocalRoot());
+              Thing* parent = Thing::LocalRoot());
 
   float touchDistance = 0.2f;
 

Messages/ThingMsg.cpp

@@ -17,19 +17,11 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
   if (thing->IsRoot())
     this->parentId = 0;
   else {
-    Thing parent = thing->GetParent();
+    Thing* parent = thing->GetParent();
-    this->parentId = parent.id;
+    this->parentId = parent->id;
   }
 }
 
-// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
-//                    unsigned char thingType, unsigned char parentId) {
-//   this->networkId = networkId;
-//   this->thingId = thingId;
-//   this->thingType = thingType;
-//   this->parentId = parentId;
-// }
-
 ThingMsg::~ThingMsg() {}
 
 unsigned char ThingMsg::Serialize(char* buffer) {

Participant.cpp

@@ -8,14 +8,26 @@ namespace RoboidControl {
 
 ParticipantRegistry Participant::registry;
 
-//Participant Participant::LocalParticipant = Participant("0.0.0.0", 0);
+Participant* Participant::LocalParticipant = new Participant();
 
-Participant& Participant::LocalParticipant() {
+void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
-    static Participant localParticipant("0.0.0.0", 0);
+  LocalParticipant = &newParticipant;
-    return localParticipant;
+  std::cout << "Replaced local participant" << std::endl;
-  }
+}
+
+Participant::Participant() {
+  std::cout << "P\n";
+  //this->root.name = "Isolated";
+  this->root = new Thing(this);
+  this->root->name = "Root";
+  this->Add(this->root);
+}
 
 Participant::Participant(const char* ipAddress, int port) {
+  // Add the root thing to the list of things, because we could not do that
+  // earlier (I think)
+  this->Add(this->root);
+
   // make a copy of the ip address string
   int addressLength = (int)strlen(ipAddress);
   int stringLength = addressLength + 1;
@@ -33,7 +45,7 @@ Participant::Participant(const char* ipAddress, int port) {
 }
 
 Participant::~Participant() {
-  registry.Remove(this);
+  // registry.Remove(this);
   delete[] this->ipAddress;
 }
 
@@ -56,9 +68,7 @@ Thing* Participant::Get(unsigned char thingId) {
 }
 
 void Participant::Add(Thing* thing, bool checkId) {
-  
   if (checkId && thing->id == 0) {
-    
     // allocate a new thing ID
 #if defined(NO_STD)
     thing->id = this->thingCount + 1;
@@ -128,8 +138,8 @@ Participant* ParticipantRegistry::Get(const char* ipAddress,
       continue;
     if (strcmp(participant->ipAddress, ipAddress) == 0 &&
         participant->port == port) {
-      std::cout << "found participant " << participant->ipAddress << ":"
+      // std::cout << "found participant " << participant->ipAddress << ":"
-                << (int)participant->port << std::endl;
+      //           << (int)participant->port << std::endl;
       return participant;
     }
   }
@@ -165,19 +175,22 @@ void ParticipantRegistry::Add(Participant* participant) {
 
   if (foundParticipant == nullptr) {
 #if defined(NO_STD)
-    //this->things[this->thingCount++] = thing;
+    // this->things[this->thingCount++] = thing;
 #else
     ParticipantRegistry::participants.push_back(participant);
 #endif
-  //   std::cout << "Add participant " << participant->ipAddress << ":"
+    //   std::cout << "Add participant " << participant->ipAddress << ":"
-  //             << participant->port << "[" << (int)participant->networkId
+    //             << participant->port << "[" << (int)participant->networkId
-  //             << "]\n";
+    //             << "]\n";
-  //   std::cout << "participants " << ParticipantRegistry::participants.size()
+    //   std::cout << "participants " <<
-  //             << "\n";
+    //   ParticipantRegistry::participants.size()
-  // } else {
+    //             << "\n";
-  //   std::cout << "Did not add, existing participant " << participant->ipAddress
+    // } else {
-  //             << ":" << participant->port << "[" << (int)participant->networkId
+    //   std::cout << "Did not add, existing participant " <<
-  //             << "]\n";
+    //   participant->ipAddress
+    //             << ":" << participant->port << "[" <<
+    //             (int)participant->networkId
+    //             << "]\n";
   }
 }
 

Participant.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include "Thing.h"
 
 namespace RoboidControl {
@@ -36,6 +37,7 @@ class ParticipantRegistry {
  public:
   Participant** GetAll() const;
   int count = 0;
+
  private:
   Participant** participants;
 #else
@@ -43,6 +45,7 @@ class ParticipantRegistry {
   /// @brief Get all participants
   /// @return All participants
   const std::list<Participant*>& GetAll() const;
+
  private:
   /// @brief The list of known participants
   std::list<Participant*> participants;
@@ -57,6 +60,9 @@ class ParticipantRegistry {
 /// reference to remote participants.
 class Participant {
  public:
+  /// @brief The name of the participant
+  const char* name = "Participant";
+
   /// @brief The Ip Address of a participant.
   const char* ipAddress = "0.0.0.0";
   /// @brief The port number for UDP communication with the participant.
@@ -65,6 +71,7 @@ class Participant {
   /// @brief The network Id to identify the participant
   unsigned char networkId = 0;
 
+  Participant();
   /// @brief Create a new participant with the given communcation info
   /// @param ipAddress The IP address of the participant
   /// @param port The UDP port of the participant
@@ -72,7 +79,10 @@ class Participant {
   /// @brief Destructor for the participant
   ~Participant();
 
-  static Participant& LocalParticipant();
+  static Participant* LocalParticipant;
+  static void ReplaceLocalParticipant(Participant& newParticipant);
+
+  Thing* root = new Thing(this);
 
  public:
 #if defined(NO_STD)

Participants/ParticipantUDP.cpp

@@ -1,5 +1,6 @@
 #include "ParticipantUDP.h"
 
+#include "Participant.h"
 #include "Thing.h"
 
 #include "Arduino/ArduinoParticipant.h"
@@ -11,20 +12,38 @@
 
 namespace RoboidControl {
 
+#pragma region Init
+
 ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
+  this->name = "ParticipantUDP";
   this->remoteSite = nullptr;
   if (this->port == 0)
     this->isIsolated = true;
   Participant::registry.Add(this);
+
+  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
 }
 
 ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
     : Participant("127.0.0.1", localPort) {
+  this->name = "ParticipantUDP";
   if (this->port == 0)
     this->isIsolated = true;
   else
     this->remoteSite = new Participant(ipAddress, port);
   Participant::registry.Add(this);
+
+  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
 }
 
 static ParticipantUDP* isolatedParticipant = nullptr;
@@ -64,11 +83,19 @@ void ParticipantUDP::SetupUDP(int localPort,
   this->connected = true;
 }
 
+#pragma endregion Init
+
 #pragma region Update
 
+// The update order
+// 1. receive external messages
+// 2. update the state
+// 3. send out the updated messages
 void ParticipantUDP::Update() {
   unsigned long currentTimeMs = Thing::GetTimeMs();
 
+  PrepMyThings();
+
   if (this->isIsolated == false) {
     if (this->connected == false)
       begin();
@@ -91,11 +118,22 @@ void ParticipantUDP::Update() {
   UpdateOtherThings();
 }
 
+void ParticipantUDP::PrepMyThings() {
+  for (Thing* thing : this->things) {
+    if (thing == nullptr)
+      continue;
+      
+    thing->PrepareForUpdate();
+  }
+}
+
 void ParticipantUDP::UpdateMyThings() {
+  // std::cout << this->things.size() << std::endl;
   for (Thing* thing : this->things) {
     if (thing == nullptr)  // || thing->GetParent() != nullptr)
       continue;
 
+    // std::cout << thing->name << "\n";
     if (thing->hierarchyChanged) {
       if (!(this->isIsolated || this->networkId == 0)) {
         ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
@@ -110,12 +148,14 @@ void ParticipantUDP::UpdateMyThings() {
       }
     }
 
+    // std::cout << "B\n";
     // Why don't we do recursive?
     // Because when a thing creates a thing in the update,
     // that new thing is not sent out (because of hierarchyChanged)
     // before it is updated itself: it is immediatedly updated!
     thing->Update(false);
 
+    // std::cout << "C\n";
     if (!(this->isIsolated || this->networkId == 0)) {
       if (thing->terminate) {
         DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
@@ -136,8 +176,10 @@ void ParticipantUDP::UpdateMyThings() {
         delete binaryMsg;
       }
     }
+    // std::cout << "D\n";
     if (thing->terminate)
       this->Remove(thing);
+    // std::cout << "E\n";
   }
 }
 
@@ -200,8 +242,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
   if (bufferSize <= 0)
     return true;
 
-  // std::cout << "send msg " << (int)this->buffer[0] << " to "
+  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-  // << remoteParticipant->ipAddress << std::endl;
+  //           << " to " << remoteParticipant->ipAddress << std::endl;
 
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
@@ -378,14 +420,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
 
 void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
 #if defined(DEBUG)
-  std::cout << this->name << ": process SiteMsg " << (int)this->networkId
+  std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
             << " -> " << (int)msg->networkId << "\n";
 #endif
 
   if (this->networkId != msg->networkId) {
     this->networkId = msg->networkId;
 
-    // std::cout << this->things.size() << " things\n";
+    std::cout << this->things.size() << " things\n";
     for (Thing* thing : this->things)
       this->SendThingInfo(sender, thing);
   }

Participants/ParticipantUDP.h

@@ -78,9 +78,6 @@ class ParticipantUDP : public Participant {
   /// local network
   long publishInterval = 3000;  // 3 seconds
 
-  /// @brief The name of the participant
-  const char* name = "ParticipantUDP";
-
  protected:
   char buffer[1024];
 
@@ -105,6 +102,8 @@ class ParticipantUDP : public Participant {
  protected:
   unsigned long nextPublishMe = 0;
 
+  /// @brief Prepare the local things for the next update
+  virtual void PrepMyThings();
   virtual void UpdateMyThings();
   virtual void UpdateOtherThings();
 

Participants/SiteServer.cpp

@@ -73,10 +73,13 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->thingId);
   if (thing == nullptr)
     // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
-    Thing::Reconstruct(sender, msg->thingType, msg->thingId);
+    // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
+    //thing = new Thing(msg->thingType, sender->root);
+    ;
+  thing->id = msg->thingId;
 
   if (msg->parentId != 0) {
-    thing->SetParent(*Get(msg->parentId));
+    thing->SetParent(Get(msg->parentId));
     if (thing->IsRoot())
     // if (thing->GetParent() != nullptr)
 #if defined(NO_STD)

Thing.cpp

@@ -20,12 +20,31 @@ namespace RoboidControl {
 
 #pragma region Init
 
-Thing& Thing::LocalRoot() {
+Thing* Thing::LocalRoot() {
-  static Thing localRoot(Thing::Type::Undetermined, localRoot);
+  Participant* p = Participant::LocalParticipant;
+  Thing* localRoot = p->root;
   return localRoot;
 }
 
-Thing::Thing(unsigned char thingType, Thing& parent) {
+// Only use this for root things
+Thing::Thing(Participant* owner) {
+  this->type = Type::Roboid;  // should become root
+
+  this->position = Spherical::zero;
+  this->positionUpdated = true;
+  this->orientation = SwingTwist::identity;
+  this->orientationUpdated = true;
+  this->hierarchyChanged = true;
+
+  this->linearVelocity = Spherical::zero;
+  this->angularVelocity = Spherical::zero;
+
+  this->owner = owner;
+  //this->owner->Add(this, true);
+  std::cout << this->owner->name << ": New root thing " << std::endl;
+}
+
+Thing::Thing(unsigned char thingType, Thing* parent) {
   this->type = thingType;
 
   this->position = Spherical::zero;
@@ -37,9 +56,16 @@ Thing::Thing(unsigned char thingType, Thing& parent) {
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
 
-  this->owner = &Participant::LocalParticipant();
+  this->owner = parent->owner;
   this->owner->Add(this, true);
   this->SetParent(parent);
+
+  std::cout << this->owner->name << ": New thing for " << parent->name
+            << std::endl;
+}
+
+Thing::~Thing() {
+  std::cout << "Destroy thing " << this->name << std::endl;
 }
 
 // Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
@@ -66,28 +92,28 @@ void Thing::SetModel(const char* url) {
 
 #pragma region Hierarchy
 
-// void Thing::SetParent(Thing* parent) {
+void Thing::SetParent(Thing* parent) {
-//   if (parent == nullptr) {
+  if (parent == nullptr) {
-//     Thing* parentThing = this->parent;
+    Thing* parentThing = this->parent;
-//     if (parentThing != nullptr)
+    if (parentThing != nullptr)
-//       parentThing->RemoveChild(this);
+      parentThing->RemoveChild(this);
-//     this->parent = nullptr;
+    this->parent = nullptr;
-//   } else
+  } else
-//     parent->AddChild(this);
+    parent->AddChild(this);
-//   this->hierarchyChanged = true;
-// }
-
-void Thing::SetParent(Thing& parent) {
-  parent.AddChild(this);
   this->hierarchyChanged = true;
 }
 
-const Thing& Thing::GetParent() {
+// void Thing::SetParent(Thing* parent) {
-  return *this->parent;
+//   parent->AddChild(this);
-}
+//   this->hierarchyChanged = true;
+// }
+
+// const Thing& Thing::GetParent() {
+//   return *this->parent;
+// }
 
 bool Thing::IsRoot() const {
-  return this == &LocalRoot();  //&Thing::Root;
+  return this == LocalRoot() || this->parent == nullptr;  //&Thing::Root;
 }
 
 // void Thing::SetParent(Thing* root, const char* name) {
@@ -96,9 +122,9 @@ bool Thing::IsRoot() const {
 //     this->SetParent(thing);
 // }
 
-// Thing* Thing::GetParent() {
+Thing* Thing::GetParent() {
-//   return this->parent;
+  return this->parent;
-// }
+}
 
 Thing* Thing::GetChildByIndex(unsigned char ix) {
   return this->children[ix];
@@ -110,11 +136,11 @@ void Thing::AddChild(Thing* child) {
 
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
     newChildren[childIx] = this->children[childIx];
-    // if (this->children[childIx] == child) {
+    if (this->children[childIx] == child) {
-    //   // child is already present, stop copying do not update the children
+      // child is already present, stop copying do not update the children
-    //   delete[] newChildren;
+      delete[] newChildren;
-    //   return;
+      return;
-    // }
+    }
   }
 
   newChildren[this->childCount] = child;
@@ -143,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) {
     }
   }
 
-  child->parent = &Thing::LocalRoot();
+  child->parent = Thing::LocalRoot();
 
   delete[] this->children;
   this->children = newChildren;
@@ -248,6 +274,8 @@ unsigned long Thing::GetTimeMs() {
 //   Update(GetTimeMs(), recursive);
 // }
 
+void Thing::PrepareForUpdate() {}
+
 void Thing::Update(bool recursive) {
   // if (this->positionUpdated || this->orientationUpdated)
   //   OnPoseChanged callback

Thing.h

@@ -41,8 +41,15 @@ class Thing {
   };
 
 #pragma region Init
-  static Thing& LocalRoot();
+  static Thing* LocalRoot();
 
+ private:
+  // Special constructor to create a root thing
+  Thing(Participant* parent);
+  // Which can only be used by the Participant
+  friend class Participant;
+
+ public:
   /// @brief Create a new thing
   /// @param thingType The type of thing (can use Thing::Type)
   /// @param parent (optional) The parent thing
@@ -50,7 +57,7 @@ class Thing {
   /// be Participant::LocalParticipant if the parent is not specified. A thing
   /// without a parent will be a root thing.
   Thing(unsigned char thingType = Thing::Type::Undetermined,
-        Thing& parent = LocalRoot());
+        Thing* parent = LocalRoot());
 
   /// @brief Create a new child thing
   /// @param parent The parent thing
@@ -59,6 +66,8 @@ class Thing {
   /// an ID
   /// @note The owner will be the same as the owner of the parent thing
 
+  ~Thing();
+
   static Thing Reconstruct(Participant* owner,
                            unsigned char thingType,
                            unsigned char thingId);
@@ -73,6 +82,7 @@ class Thing {
 
   /// @brief The participant managing this thing
   Participant* owner = nullptr;
+
   /// @brief The ID of the thing
   unsigned char id = 0;
 
@@ -108,11 +118,11 @@ class Thing {
   /// @brief Sets the parent of this Thing
   /// @param parent The Thing which should become the parent
   // virtual void SetParent(Thing* parent);
-  void SetParent(Thing& parent);
+  void SetParent(Thing* parent);
   /// @brief Gets the parent of this Thing
   /// @return The parent Thing
   // Thing* GetParent();
-  const Thing& GetParent();
+  Thing* GetParent();
 
   bool IsRoot() const;
 
@@ -215,12 +225,7 @@ class Thing {
 #pragma region Update
 
  public:
-  /// @brief Get the current time in milliseconds
+  virtual void PrepareForUpdate();
-  /// @return The current time in milliseconds
-  static unsigned long GetTimeMs();
-
-  /// @brief Updates the state of the thing
-  // void Update(bool recursive = false);
 
   /// @brief Updates the state of the thing
   /// @param currentTimeMs The current clock time in milliseconds; if this is
@@ -230,6 +235,10 @@ class Thing {
 
   static void UpdateThings();
 
+  /// @brief Get the current time in milliseconds
+  /// @return The current time in milliseconds
+  static unsigned long GetTimeMs();
+
 #pragma endregion Update
 
  public:

Things/ControlledMotor.cpp

@@ -2,9 +2,9 @@
 
 namespace RoboidControl {
 
-ControlledMotor::ControlledMotor(Motor& motor,
+ControlledMotor::ControlledMotor(Motor* motor,
-                                 RelativeEncoder& encoder,
+                                 RelativeEncoder* encoder,
-                                 Thing& parent)
+                                 Thing* parent)
     : Motor(parent), motor(motor), encoder(encoder) {
   this->type = Type::ControlledMotor;
   // encoder.SetParent(*this);
@@ -19,9 +19,9 @@ void ControlledMotor::SetTargetVelocity(float velocity) {
 }
 
 void ControlledMotor::Update(bool recurse) {
-  encoder.Update(false);
+  encoder->Update(false);
 
-  this->actualVelocity = (int)rotationDirection * encoder.rotationSpeed;
+  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
 
   unsigned long currentTimeMs = GetTimeMs();
   float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
@@ -31,10 +31,10 @@ void ControlledMotor::Update(bool recurse) {
   float acceleration =
       error * timeStep * pidP;  // Just P is used at this moment
   std::cout << "motor acc. " << acceleration << std::endl;
-  motor.SetTargetVelocity(targetVelocity +
+  motor->SetTargetVelocity(targetVelocity +
                           acceleration);  // or something like that
 
-  motor.Update(false);
+  motor->Update(false);
 }
 
 // float ControlledMotor::GetActualVelocity() {

Things/ControlledMotor.h

@@ -10,7 +10,7 @@ namespace RoboidControl {
 /// The speed is measured in revolutions per second.
 class ControlledMotor : public Motor {
  public:
-  ControlledMotor(Motor& motor, RelativeEncoder& encoder, Thing& parent = Thing::LocalRoot());
+  ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
 
   float pidP = 1;
   float pidD = 0;
@@ -28,8 +28,8 @@ class ControlledMotor : public Motor {
   /// @param speed the target velocity in revolutions per second
   virtual void SetTargetVelocity(float velocity) override;
 
-  Motor& motor;
+  Motor* motor;
-  RelativeEncoder& encoder;
+  RelativeEncoder* encoder;
 
  protected:
   float lastUpdateTime;

Things/DifferentialDrive.cpp

@@ -1,11 +1,27 @@
 #include "DifferentialDrive.h"
 
+#include "Messages/LowLevelMessages.h"
+
 namespace RoboidControl {
 
-DifferentialDrive::DifferentialDrive(Thing& parent)
+DifferentialDrive::DifferentialDrive(Thing* parent)
     : Thing(Type::DifferentialDrive, parent) {
-  this->leftWheel = new Motor();
+  this->name = "Differential drive";
-  this->rightWheel = new Motor();
+
+  this->leftWheel = new Motor(this);
+  this->leftWheel->name = "Left motor";
+
+  this->rightWheel = new Motor(this);
+  this->rightWheel->name = "Right motor";
+}
+
+DifferentialDrive::DifferentialDrive(Motor* leftMotor,
+                                     Motor* rightMotor,
+                                     Thing* parent)
+    : Thing(Type::DifferentialDrive, parent) {
+  this->name = "Differential drive";
+  this->leftWheel = leftMotor;
+  this->rightWheel = rightMotor;
 }
 
 void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
@@ -23,13 +39,13 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-Motor& DifferentialDrive::GetMotorLeft() {
+// Motor& DifferentialDrive::GetMotorLeft() {
-  return *this->leftWheel;
+//   return *this->leftWheel;
-}
+// }
 
-Motor& DifferentialDrive::GetMotorRight() {
+// Motor& DifferentialDrive::GetMotorRight() {
-  return *this->rightWheel;
+//   return *this->rightWheel;
-}
+// }
 
 void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
   float distance = this->wheelSeparation / 2;
@@ -78,4 +94,11 @@ void DifferentialDrive::Update(bool recursive) {
   Thing::Update(recursive);
 }
 
+int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = this->leftWheel->id;
+  data[(*ix)++] = this->rightWheel->id;
+  LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
+  return 4;
+}
+
 }  // namespace RoboidControl

Things/DifferentialDrive.h

@@ -14,7 +14,9 @@ class DifferentialDrive : public Thing {
   /// @param parent The parent thing
   /// @param thingId The ID of the thing, leave out or set to zero to generate
   /// an ID
-  DifferentialDrive(Thing& parent = Thing::LocalRoot());
+  DifferentialDrive(Thing* parent = Thing::LocalRoot());
+
+  DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot());
 
   /// @brief Configures the dimensions of the drive
   /// @param wheelDiameter The diameter of the wheels in meters
@@ -25,8 +27,8 @@ class DifferentialDrive : public Thing {
   /// @sa SetLinearVelocity SetAngularVelocity
   void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
 
-  Motor& GetMotorLeft();
+  // Motor& GetMotorLeft();
-  Motor& GetMotorRight();
+  // Motor& GetMotorRight();
   /// @brief Congures the motors for the wheels
   /// @param leftWheel The motor for the left wheel
   /// @param rightWheel The motor for the right wheel
@@ -42,20 +44,23 @@ class DifferentialDrive : public Thing {
   /// @copydoc RoboidControl::Thing::Update(unsigned long)
   virtual void Update(bool recursive = true) override;
 
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  // virtual void ProcessBinary(char* bytes) override;
+
+    /// @brief The left wheel
+  Motor* leftWheel = nullptr;
+  /// @brief The right wheel
+  Motor* rightWheel = nullptr;
 
  protected:
   /// @brief The radius of a wheel in meters
-  float wheelRadius = 1.0f;
+  float wheelRadius = 0.0f;
   /// @brief The distance between the wheels in meters
-  float wheelSeparation = 1.0f;
+  float wheelSeparation = 0.0f;
 
   /// @brief Convert revolutions per second to meters per second
   float rpsToMs = 1.0f;
 
-  /// @brief The left wheel
-  Motor* leftWheel = nullptr;
-  /// @brief The right wheel
-  Motor* rightWheel = nullptr;
 };
 
 }  // namespace RoboidControl

Things/DigitalSensor.cpp

@@ -13,7 +13,7 @@ namespace RoboidControl {
 // DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {}
 
 // DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {}
-DigitalSensor::DigitalSensor(Thing& parent) : Thing(Type::Switch, parent) {}
+DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {}
 
 int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
   bytes[(*ix)++] = state ? 1 : 0;

Things/DigitalSensor.h

@@ -19,7 +19,7 @@ class DigitalSensor : public Thing {
   /// @param thingId The ID of the thing, leave out or set to zero to generate
   /// an ID
   // DigitalSensor(Thing* parent); //, unsigned char thingId = 0);
-  DigitalSensor(Thing& parent = Thing::LocalRoot());
+  DigitalSensor(Thing* parent = Thing::LocalRoot());
 
   /// @brief The sigital state
   bool state = 0;

Things/Motor.cpp

@@ -2,16 +2,15 @@
 
 namespace RoboidControl {
 
-// RoboidControl::Motor::Motor(Participant* owner)
+Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
-//     : Thing(owner, Type::UncontrolledMotor) {}
 
-// RoboidControl::Motor::Motor(Thing* parent)
+void Motor::SetTargetVelocity(float targetSpeed) {
-//     : Thing(parent, Type::UncontrolledMotor) {}
-
-Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {}
-
-void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) {
     this->targetVelocity = targetSpeed;
 }
 
+int Motor::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = this->targetVelocity * 127.0f;
+  return 1;
+}
+
 }  // namespace RoboidControl

Things/Motor.h

@@ -6,18 +6,19 @@ namespace RoboidControl {
 
 class Motor : public Thing {
  public:
-  Motor(Participant* owner);
+  Motor(Thing* parent = Thing::LocalRoot());
-  // Motor(Thing* parent);
-  Motor(Thing& parent = Thing::LocalRoot());
 
   /// @brief Motor turning direction
   enum class Direction { Clockwise = 1, CounterClockwise = -1 };
   /// @brief The forward turning direction of the motor
   Direction direction;
 
-  virtual void SetTargetVelocity(float velocity); // -1..0..1
+  virtual void SetTargetVelocity(float velocity);  // -1..0..1
 
- protected:
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  // virtual void ProcessBinary(char* bytes) override;
+
+ //protected:
   float targetVelocity = 0;
 };
 

Things/RelativeEncoder.cpp

@@ -2,7 +2,7 @@
 
 namespace RoboidControl {
 
-RelativeEncoder::RelativeEncoder(Thing& parent)
+RelativeEncoder::RelativeEncoder(Thing* parent)
     : Thing(Type::IncrementalEncoder, parent) {}
 
 float RelativeEncoder::GetRotationSpeed() {

Things/RelativeEncoder.h

@@ -16,7 +16,7 @@ class RelativeEncoder : public Thing {
   /// rotation
   RelativeEncoder(Participant* owner);
   // RelativeEncoder(Thing* parent);
-  RelativeEncoder(Thing& parent = Thing::LocalRoot());
+  RelativeEncoder(Thing* parent = Thing::LocalRoot());
 
   /// @brief Get the rotation speed 
   /// @return The speed in revolutions per second

Things/TemperatureSensor.cpp

@@ -10,7 +10,7 @@ namespace RoboidControl {
 
 // TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
 
-TemperatureSensor::TemperatureSensor(Thing& parent) : Thing(Type::TemperatureSensor, parent) {}
+TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
 
 // TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}
 

Things/TemperatureSensor.h

@@ -14,7 +14,7 @@ class TemperatureSensor : public Thing {
   /// @param thingId The ID of the thing
   TemperatureSensor(Participant* participant); //, unsigned char thingId);
   // TemperatureSensor(Thing* parent);
-  TemperatureSensor(Thing& parent = Thing::LocalRoot());
+  TemperatureSensor(Thing* parent = Thing::LocalRoot());
 
       /// @brief The measured temperature
       float temperature = 0;

Things/TouchSensor.cpp

@@ -2,15 +2,29 @@
 
 namespace RoboidControl {
 
-TouchSensor::TouchSensor(Thing& parent) : Thing(Type::TouchSensor, parent) {}
+TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) {
+  this->name = "Touch sensor";
+}
+
+void TouchSensor::PrepareForUpdate() {
+  this->touchedSomething = this->externalTouch;
+}
+
+void TouchSensor::Update(bool recursive) {
+  Thing::Update(recursive);
+}
 
 int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  bytes[(*ix)++] = touchedSomething ? 1 : 0;
+  bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
   return 1;
 }
 
 void TouchSensor::ProcessBinary(char* bytes) {
-  this->touchedSomething |= (bytes[0] == 1);
+  this->externalTouch = (bytes[0] == 1);
+  if (this->externalTouch)
+    std::cout << "touching!\n";
+  else
+    std::cout << "not touching\n";
 }
 
 }  // namespace RoboidControl

Things/TouchSensor.h

@@ -12,12 +12,15 @@ class TouchSensor : public Thing {
   /// @param parent The parent thing
   /// @param thingId The ID of the thing, leave out or set to zero to generate
   /// an ID
-  TouchSensor(Thing& parent = Thing::LocalRoot());
+  TouchSensor(Thing* parent = Thing::LocalRoot());
 
   /// @brief Value which is true when the sensor is touching something, false
   /// otherwise
   bool touchedSomething = false;
 
+  virtual void PrepareForUpdate() override;
+  virtual void Update(bool recursive) override;
+
   /// @brief Function used to generate binary data for this touch sensor
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
@@ -25,6 +28,8 @@ class TouchSensor : public Thing {
   /// @brief Function used to process binary data received for this touch sensor
   /// @param bytes The binary data to process
   virtual void ProcessBinary(char* bytes) override;
+protected:
+  bool externalTouch = false;
 };
 
 }  // namespace RoboidControl