Merge commit 'b6fbfd92d26c91b680fb6c43da2db19adec82376'

Arduino/ArduinoParticipant.cpp

@@ -1,5 +1,9 @@
 #include "ArduinoParticipant.h"
 
+#if !defined(NO_STD)
+#include <iostream>
+#endif
+
 #if defined(ARDUINO)
 #if defined(ARDUINO_ARCH_ESP8266)
 #include <ESP8266WiFi.h>
@@ -19,17 +23,19 @@
 namespace RoboidControl {
 namespace Arduino {
 
-void ParticipantUDP::Setup(int localPort,
-                             const char* remoteIpAddress,
-                             int remotePort) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  this->remoteIpAddress = remoteIpAddress;
+WiFiUDP* udp;
-  this->remotePort = remotePort;
+#endif
+
+void ParticipantUDP::Setup() {
+#if defined(ARDUINO) && defined(HAS_WIFI)
   GetBroadcastAddress();
 
 #if defined(UNO_R4)
   if (WiFi.status() == WL_NO_MODULE) {
+#if !defined(NO_STD)
     std::cout << "No network available!\n";
+#endif
     return;
   }
 #else
@@ -38,9 +44,16 @@ void ParticipantUDP::Setup(int localPort,
     return;
   }
 #endif
-  udp.begin(localPort);
 
-  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
+  udp = new WiFiUDP();
+  udp->begin(this->port);
+
+#if !defined(NO_STD)
+  std::cout << "Wifi sync started local " << this->port;
+  if (this->remoteSite != nullptr)
+    std::cout << ", remote " << this->remoteSite->ipAddress << ":"
+              << this->remoteSite->port << "\n";
+#endif
 #endif
 }
 
@@ -52,36 +65,25 @@ void ParticipantUDP::GetBroadcastAddress() {
   this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
   broadcastIpString.toCharArray(this->broadcastIpAddress,
                                 broadcastIpString.length() + 1);
+#if !defined(NO_STD)
   std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
 #endif
+#endif
 }
 
 void ParticipantUDP::Receive() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  int packetSize = udp.parsePacket();
+  int packetSize = udp->parsePacket();
   while (packetSize > 0) {
-    udp.read(buffer, packetSize);
+    udp->read(buffer, packetSize);
 
-    String senderAddress = udp.remoteIP().toString();
+    String senderAddress = udp->remoteIP().toString();
     char sender_ipAddress[16];
     senderAddress.toCharArray(sender_ipAddress, 16);
-    unsigned int sender_port = udp.remotePort();
+    unsigned int sender_port = udp->remotePort();
 
-    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
-    // sender_port); if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(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();
+    packetSize = udp->parsePacket();
   }
 #endif
 }
@@ -92,15 +94,22 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
   // << remoteParticipant->port << "\n";
 
   int n = 0;
+  int r = 0;
   do {
     if (n > 0) {
+#if !defined(NO_STD)
       std::cout << "Retry sending\n";
+#endif
       delay(10);
     }
     n++;
-    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
+
-    udp.write((unsigned char*)buffer, bufferSize);
+    udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
-  } while (udp.endPacket() == 0 && n < 10);
+    udp->write((unsigned char*)buffer, bufferSize);
+    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;
@@ -112,9 +121,9 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   if (bufferSize <= 0)
     return true;
 
-  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
+  udp->beginPacket(this->broadcastIpAddress, this->port);
-  udp.write((unsigned char*)buffer, bufferSize);
+  udp->write((unsigned char*)buffer, bufferSize);
-  udp.endPacket();
+  udp->endPacket();
 
   //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
   //             << this->remotePort << "\n";

Arduino/ArduinoParticipant.h

@@ -2,29 +2,19 @@
 
 #include "Participants/ParticipantUDP.h"
 
-#if defined(HAS_WIFI)
-#include <WiFiUdp.h>
-#endif
-
 namespace RoboidControl {
 namespace Arduino {
 
 class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
-  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Setup();
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
-#if defined(HAS_WIFI)
-  const char* remoteIpAddress = nullptr;
-  unsigned short remotePort = 0;
   char* broadcastIpAddress = nullptr;
 
-  WiFiUDP udp;
-#endif
-
   void GetBroadcastAddress();
 };
 

Arduino/Examples/BB2A/BB2A.cpp

@@ -0,0 +1,95 @@
+#include "Arduino.h"
+
+#include "Things/ControlledMotor.h"
+#include "Things/DifferentialDrive.h"
+#include "Things/RelativeEncoder.h"
+#include "Things/TouchSensor.h"
+
+#include "Arduino/Things/DRV8833.h"
+#include "Arduino/Things/DigitalInput.h"
+#include "Arduino/Things/UltrasonicSensor.h"
+#include "Arduino/ArduinoUtils.h"
+
+#include "Participants/ParticipantUDP.h"
+
+#include "configuration.h"
+
+#include <iostream>
+
+using namespace RoboidControl;
+using namespace RoboidControl::Arduino;
+
+ParticipantUDP* localParticipant;
+
+DifferentialDrive* bb2b;
+TouchSensor* touchLeft;
+TouchSensor* touchRight;
+
+// RelativeEncoder* encoderLeft;
+// RelativeEncoder* encoderRight;
+
+void setup() {
+  Serial.begin(115200);
+  delay(3000);
+  Serial.println("started");
+
+  StartWifi("serrarens", "192.168.76.44");
+  localParticipant = new ParticipantUDP("192.168.77.76");
+
+  bb2b = new DifferentialDrive();
+  touchLeft = new TouchSensor(bb2b);
+  touchRight = new TouchSensor(bb2b);
+  
+  // bb2b = new DRV8833::DifferentialDrive(driveConfig);
+  // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b);
+  // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b);
+
+  touchLeft->name = "Left Touch Sensor";
+  touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0));
+  touchRight->name = "Right Touch Sensor";
+  touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0));
+
+  // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig);
+  // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig);
+
+  // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b);
+  // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b);
+}
+
+void loop() {
+  
+  // std::cout << touchLeft->touchedSomething << " | "
+  //           << touchRight->touchedSomething << std::endl;
+  // std::cout << encoderLeft->rotationSpeed << " : "
+  //           << encoderRight->rotationSpeed << std::endl;
+  // std::cout << bb2b->leftWheel->encoder->rotationSpeed
+  //           << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl;
+
+  
+  // The left wheel turns forward when nothing is touched on the right side
+  // and turn backward when the roboid hits something on the right
+  float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f;
+  // The right wheel does the same, but instead is controlled by
+  // touches on the left side
+  float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f;
+  // When both sides are touching something, both wheels will turn backward
+  // and the roboid will move backwards
+
+  bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity);
+  bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity);
+  // std::cout << "  " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl;
+
+  // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f;
+  // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f;
+  // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity);
+  // std::cout << "  " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl;
+
+  // std::cout << leftMotor->actualVelocity << std::endl;
+  
+  //Serial.println(".");
+  // Update the roboid state
+  localParticipant->Update();
+
+  // and sleep for 100ms
+  delay(10);
+}

Arduino/Examples/BB2A/configuration.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "Arduino/Things/UltrasonicSensor.h"
+#include "Arduino/Things/DRV8833.h"
+#include "Arduino/Things/DigitalInput.h"
+
+using namespace RoboidControl::Arduino;
+
+#if defined(ESP32)
+static constexpr DRV8833::Configuration driveConfig = {
+    17,  // AIn1
+    16,  // AIn2
+    14,  // BIn1
+    27   // BIn2
+};
+static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
+    25,  // Trigger
+    33   // Echo
+};
+static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
+    15,  // Trigger
+    5    // Echo
+};
+
+#elif defined(UNO) || defined(UNO_R4)
+static constexpr DRV8833::Configuration driveConfig = {
+    5,  // AIn1
+    6,  // AIn2
+    7,  // BIn1
+    10  // BIn2
+};
+
+static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
+    A0,  // Trigger
+    12   // Echo
+};
+static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
+    A1,  // Trigger
+    11   // Echo
+};
+
+static constexpr DigitalInput::RelativeEncoder::Configuration
+    leftEncoderConfig = {
+        2,  // Input pin
+        80  // Pulses per revolution
+};
+static constexpr DigitalInput::RelativeEncoder::Configuration
+    rightEncoderConfig = {
+        3,  // Input pin
+        80  // Pulses per revolution
+};
+#endif

Arduino/Things/DRV8833.cpp

@@ -5,49 +5,83 @@
 namespace RoboidControl {
 namespace Arduino {
 
-DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
+#pragma region DRV8833
-    : Thing(participant) {
+
+DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
+  this->type = Type::Undetermined;
+  this->name = "DRV8833";
+  this->pinStandby = config.standby;
+  if (pinStandby != 255)
+    pinMode(pinStandby, OUTPUT);
+
+  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->SetName("Motor B");
+}
+
+#pragma endregion DRV8833
+
+#pragma region Differential drive
+
+DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
+                                              Thing* parent)
+    : RoboidControl::DifferentialDrive(nullptr, nullptr, parent) {
+  this->drv8833 = new DRV8833(config, this);
+  this->leftWheel = this->drv8833->motorA;
+  this->rightWheel = this->drv8833->motorB;
+}
+
+void DRV8833::DifferentialDrive::Update(bool recurse) {
+  RoboidControl::DifferentialDrive::Update(recurse);
+  this->drv8833->Update(false);
+}
+
+#pragma endregion Differential drive
+
+#pragma region Motor
+
+#if (ESP32)
+uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
+#endif
+
+DRV8833Motor::DRV8833Motor(DRV8833* driver,
+                           unsigned char pinIn1,
+                           unsigned char pinIn2,
+                           bool reverse)
+    : Motor(driver) {
+
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
 
 #if (ESP32)
-  in1Ch = nextAvailablePwmChannel++;
+  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
   ledcSetup(in1Ch, 500, 8);
   ledcAttachPin(pinIn1, in1Ch);
-  in2Ch = nextAvailablePwmChannel++;
+
+  in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
   ledcSetup(in2Ch, 500, 8);
   ledcAttachPin(pinIn2, in2Ch);
+
 #else
   pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
   pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
 #endif
 
-  this->reverse = reverse;
+  // this->reverse = reverse;
 }
 
-void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
+// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
-  this->maxRpm = rpm;
+//   this->maxRpm = rpm;
-}
+// }
 
-void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
+void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
-  Thing::SetAngularVelocity(velocity);
+  Motor::SetTargetVelocity(motorSpeed);
-  // ignoring rotation axis for now.
-  // Spherical angularVelocity = this->GetAngularVelocity();
-  float angularSpeed = velocity.distance;  // in degrees/sec
 
-  float rpm = angularSpeed / 360 * 60;
+  uint8_t motorSignal =
-  float motorSpeed = rpm / this->maxRpm;
+      (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
 
-  uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
+  // std::cout << "moto speed " << this->name << " = " << motorSpeed
-  // if (direction == RotationDirection::CounterClockwise)
-  //   speed = -speed;
-  // Determine the rotation direction
-  if (velocity.direction.horizontal.InDegrees() < 0)
-    motorSpeed = -motorSpeed;
-  if (this->reverse)
-    motorSpeed = -motorSpeed;
-
-  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
   //           << ", motor signal = " << (int)motorSignal << "\n";
 
 #if (ESP32)
@@ -97,24 +131,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
 #endif
 }
 
-DRV8833::DRV8833(Participant* participant,
+#pragma endregion Motor
-                 unsigned char pinAIn1,
-                 unsigned char pinAIn2,
-                 unsigned char pinBIn1,
-                 unsigned char pinBIn2,
-                 unsigned char pinStandby,
-                 bool reverseA,
-                 bool reverseB)
-    : Thing(participant) {
-  this->pinStandby = pinStandby;
-  if (pinStandby != 255)
-    pinMode(pinStandby, OUTPUT);
-
-  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
-  this->motorA->name = "Motor A";
-  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
-  this->motorB->name = "Motor B";
-}
 
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DRV8833.h

@@ -1,60 +1,87 @@
 #pragma once
 
+#include <Arduino.h>
+#include "Participants/IsolatedParticipant.h"
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
+#include "Things/Motor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
-/// @brief Support for a DRV8833 motor controller
+class DRV8833Motor;
-class DRV8833Motor : public Thing {
- public:
-  /// @brief Motor turning direction
-  enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
-
-  /// @brief Setup the DC 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(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false);
-  void SetMaxRPM(unsigned int rpm);
-
-  virtual void SetAngularVelocity(Spherical velocity) override;
-
-  bool reverse = false;
-
- protected:
-  unsigned char pinIn1 = 255;
-  unsigned char pinIn2 = 255;
-  unsigned int maxRpm = 200;
-};
 
 class DRV8833 : public Thing {
  public:
+  struct Configuration {
+    int AIn1;
+    int AIn2;
+    int BIn1;
+    int BIn2;
+    int standby = 255;
+
+    constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {}
+  };
+
   /// @brief Setup a DRV8833 motor controller
-  /// @param pinAIn1 The pin number connected to the AIn1 port
+  DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
-  /// @param pinAIn2 The pin number connected to the AIn2 port
-  /// @param pinBIn1 The pin number connected to the BIn1 port
-  /// @param pinBIn2 The pin number connceted to the BIn2 port
-  /// @param pinStandby The pin number connected to the standby port, 255
-  /// indicated that the port is not connected
-  /// @param reverseA The forward turning direction of motor A
-  /// @param reverseB The forward turning direction of motor B
-  DRV8833(Participant* participant,
-          unsigned char pinAIn1,
-          unsigned char pinAIn2,
-          unsigned char pinBIn1,
-          unsigned char pinBIn2,
-          unsigned char pinStandby = 255,
-          bool reverseA = false,
-          bool reverseB = false);
 
   DRV8833Motor* motorA = nullptr;
   DRV8833Motor* motorB = nullptr;
 
  protected:
   unsigned char pinStandby = 255;
+
+ public:
+  class DifferentialDrive;
 };
 
+#pragma region Differential drive
+
+class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
+ public:
+  DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
+
+  virtual void Update(bool recurse = false) override;
+
+ protected:
+  DRV8833* drv8833 = nullptr;
+};
+
+#pragma endregion Differential drive
+
+#pragma region Motor
+
+/// @brief Support for a DRV8833 motor controller
+class DRV8833Motor : public Motor {
+ public:
+  /// @brief Setup the DC 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,
+               unsigned char pinIn1,
+               unsigned char pinIn2,
+               bool reverse = false);
+  // void SetMaxRPM(unsigned int rpm);
+
+  // virtual void SetAngularVelocity(Spherical velocity) override;
+  virtual void SetTargetVelocity(float targetSpeed) override;
+  // bool reverse = false;
+
+ protected:
+  unsigned char pinIn1 = 255;
+  unsigned char pinIn2 = 255;
+  // unsigned int maxRpm = 200;
+
+#if (ESP32)
+  uint8_t in1Ch;
+  uint8_t in2Ch;
+  static uint8_t nextAvailablePwmChannel;
+#endif
+};
+
+#pragma endregion Motor
+
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.cpp

@@ -5,19 +5,126 @@
 namespace RoboidControl {
 namespace Arduino {
 
-DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
+#pragma region Digital input
-    : TouchSensor(participant) {
+
+DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) {
+  this->type = Type::Switch;
+  this->name = "Digital Input";
   this->pin = pin;
-
+  pinMode(this->pin, INPUT);
-  pinMode(pin, INPUT);
+  std::cout << "digital input start\n";
 }
 
-void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
+void DigitalInput::Update(bool recursive) {
-  this->touchedSomething = digitalRead(pin) == LOW;
+  this->isHigh = digitalRead(this->pin);
-  // std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
+  this->isLow = !this->isHigh;
-  // this->touchedSomething << "\n";
+  Thing::Update(recursive);
-  Thing::Update(currentTimeMs, recursive);
 }
 
+#pragma endregion Digital input
+
+#pragma region Touch sensor
+
+DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
+    : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
+
+void DigitalInput::TouchSensor::Update(bool recursive) {
+  this->internalTouch = digitalInput.isLow;
+}
+
+#pragma endregion Touch sensor
+
+#pragma region Relative encoder
+
+int DigitalInput::RelativeEncoder::interruptCount = 0;
+volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
+volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
+
+DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
+                                               Thing* parent)
+    : RoboidControl::RelativeEncoder(parent),
+      digitalInput(config.pin, parent),
+      pulsesPerRevolution(config.pulsesPerRevolution) {}
+
+void DigitalInput::RelativeEncoder::Start() {
+  // We support up to 2 pulse counters
+  if (interruptCount == 0) {
+    std::cout << "pin interrupt 1 activated" << std::endl;
+    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
+                    RISING);
+  } else if (interruptCount == 1) {
+    std::cout << "pin interrupt 2 activated" << std::endl;
+    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1,
+                    RISING);
+  } else {
+    // maximum interrupt count reached
+    std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached"
+              << std::endl;
+    return;
+  }
+  interruptIx = interruptCount;
+  interruptCount++;
+  std::cout << "pin ints. " << interruptCount << std::endl;
+}
+
+int DigitalInput::RelativeEncoder::GetPulseCount() {
+  if (interruptIx == 0) {
+    int pulseCount = pulseCount0;
+    pulseCount0 = 0;
+    return pulseCount;
+  } else if (interruptIx == 1) {
+    int pulseCount = pulseCount1;
+    pulseCount1 = 0;
+    return pulseCount;
+  }
+  return 0;
+}
+
+void DigitalInput::RelativeEncoder::Update(bool recursive) {
+  unsigned long currentTimeMs = GetTimeMs();
+  if (this->lastUpdateTime == 0) {
+    this->Start();
+    this->lastUpdateTime = currentTimeMs;
+    return;
+  }
+
+  float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f;
+  if (timeStep <= 0)
+    return;
+
+  int pulseCount = GetPulseCount();
+  netPulseCount += pulseCount;
+
+  this->pulseFrequency = pulseCount / timeStep;
+  this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
+
+  // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
+  //           << " timestep " << timeStep << std::endl;
+
+  this->lastUpdateTime = currentTimeMs;
+}
+
+#if defined(ESP8266) || defined(ESP32)
+void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() {
+  pulseCount0++;
+}
+#else
+void DigitalInput::RelativeEncoder::PulseInterrupt0() {
+  pulseCount0++;
+}
+#endif
+
+#if defined(ESP8266) || defined(ESP32)
+void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() {
+  pulseCount1++;
+}
+#else
+void DigitalInput::RelativeEncoder::PulseInterrupt1() {
+  pulseCount1++;
+}
+#endif
+
+#pragma endregion Relative encoder
+
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.h

@@ -1,26 +1,92 @@
 #pragma once
 
+#include "Things/RelativeEncoder.h"
 #include "Things/TouchSensor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
 /// @brief A digital input represents the stat of a digital GPIO pin
-class DigitalInput : public TouchSensor {
+class DigitalInput : public Thing {
  public:
   /// @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);
+
+  bool isHigh = false;
+  bool isLow = false;
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(unsigned long currentTimeMs,
+  virtual void Update(bool recursive = false) override;
-                      bool recursive = false) override;
 
  protected:
   /// @brief The pin used for digital input
   unsigned char pin = 0;
+
+ public:
+  class TouchSensor;
+  class RelativeEncoder;
 };
 
+#pragma region Touch sensor
+
+class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
+ public:
+  TouchSensor(unsigned char pin, Thing* parent);
+
+  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
+  virtual void Update(bool recurse = false) override;
+
+ protected:
+  DigitalInput digitalInput;
+};
+
+#pragma endregion Touch sensor
+
+#pragma region Incremental encoder
+
+class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
+ public:
+  struct Configuration {
+    unsigned char pin;
+    unsigned char pulsesPerRevolution;
+  };
+
+  RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
+
+  unsigned char pulsesPerRevolution;
+
+  /// @brief The current pulse frequency in Hz
+  float pulseFrequency = 0;
+
+  /// @copydoc RoboidControl::Thing::Update()
+  virtual void Update(bool recurse = false) override;
+
+ protected:
+  DigitalInput digitalInput;
+
+  int interruptIx = 0;
+
+  static int interruptCount;
+
+  volatile static int pulseCount0;
+  static void PulseInterrupt0();
+
+  volatile static int pulseCount1;
+  static void PulseInterrupt1();
+
+  int GetPulseCount();
+  long netPulseCount = 0;
+  unsigned long lastUpdateTime = 0;
+
+ private:
+  void Start();
+};
+
+#pragma endregion Incremental encoder
+
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.cpp

@@ -1,16 +1,17 @@
 #include "UltrasonicSensor.h"
 
 #include <Arduino.h>
+#include <iostream>
 
 namespace RoboidControl {
 namespace Arduino {
 
-UltrasonicSensor::UltrasonicSensor(Participant* participant,
+UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
-                                   unsigned char pinTrigger,
+    : Thing(parent) {
-                                   unsigned char pinEcho)
+  this->type = Type::DistanceSensor;
-    : TouchSensor(participant) {
+  this->name = "Ultrasonic sensor";
-  this->pinTrigger = pinTrigger;
+  this->pinTrigger = config.trigger;
-  this->pinEcho = pinEcho;
+  this->pinEcho = config.echo;
 
   pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
   pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
@@ -19,14 +20,14 @@ UltrasonicSensor::UltrasonicSensor(Participant* participant,
 float UltrasonicSensor::GetDistance() {
   // Start the ultrasonic 'ping'
   digitalWrite(pinTrigger, LOW);
-  delayMicroseconds(5);
+  delayMicroseconds(2);
   digitalWrite(pinTrigger, HIGH);
   delayMicroseconds(10);
   digitalWrite(pinTrigger, LOW);
 
   // Measure the duration of the pulse on the echo pin
-  float duration_us =
+  unsigned long duration_us =
-      pulseIn(pinEcho, HIGH, 100000);  // the result is in microseconds
+      pulseIn(pinEcho, HIGH, 10000);  // the result is in microseconds
 
   // Calculate the distance:
   // * Duration should be divided by 2, because the ping goes to the object
@@ -37,32 +38,60 @@ float UltrasonicSensor::GetDistance() {
   // * Now we calculate the distance based on the speed of sound (340 m/s):
   // distance = duration_sec * 340;
   // * The result calculation is therefore:
-  this->distance = duration_us / 2 / 1000000 * 340;
+  this->distance = (float)duration_us / 2 / 1000000 * 340;
+
+  // Serial.println(this->distance);
+
+  // std::cout << "US distance " << this->distance << std::endl;
 
   // Filter faulty measurements. The sensor can often give values > 30 m which
   // are not correct
   // if (distance > 30)
   //   distance = 0;
 
-  this->touchedSomething |= (this->distance <= this->touchDistance);
+  return this->distance;
-
-  //   std::cout << "Ultrasonic " << this->distance << " " <<
-  //   this->touchedSomething << "\n";
-
-  return distance;
 }
 
-void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
+void UltrasonicSensor::Update(bool recursive) {
-  this->touchedSomething = false;
   GetDistance();
-  Thing::Update(currentTimeMs, recursive);
+  Thing::Update(recursive);
 }
 
-// void UltrasonicSensor::ProcessBinary(char* bytes) {
+#pragma region Distance sensor
-//   this->touchedSomething = (bytes[0] == 1);
+
-//   if (this->touchedSomething)
+UltrasonicSensor::DistanceSensor::DistanceSensor(
-//     std::cout << "Touching something!\n";
+    UltrasonicSensor::Configuration config,
-// }
+    Thing* parent)
+    : RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {}
+
+void UltrasonicSensor::DistanceSensor::Update(bool recursive) {
+  RoboidControl::DistanceSensor::Update(recursive);
+  this->ultrasonic.Update(false);
+  if (this->ultrasonic.distance > 0)
+    this->internalDistance = this->ultrasonic.distance;
+  else
+#if ARDUNIO
+    this->internalDistance = INFINITY;
+#else
+    this->internalDistance = std::numeric_limits<float>::infinity();
+#endif
+}
+
+#pragma endregion Distance sensor
+
+#pragma region Touch sensor
+
+UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
+    : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
+
+void UltrasonicSensor::TouchSensor::Update(bool recursive) {
+  RoboidControl::TouchSensor::Update(recursive);
+  this->ultrasonic.Update(false);
+  this->internalTouch = (this->ultrasonic.distance > 0 &&
+                         this->ultrasonic.distance <= this->touchDistance);
+}
+
+#pragma endregion Touch sensor
 
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.h

@@ -1,25 +1,25 @@
 #pragma once
 
+#include "Things/DistanceSensor.h"
 #include "Things/TouchSensor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
 /// @brief An HC-SR04 ultrasonic distance sensor
-class UltrasonicSensor : public TouchSensor {
+class UltrasonicSensor : Thing {
  public:
-  /// @brief  Setup an ultrasonic sensor
+  struct Configuration {
-  /// @param participant The participant to use
+    int trigger;
-  /// @param pinTrigger The pin number of the trigger signal
+    int echo;
-  /// @param pinEcho The pin number of the echo signal
+  };
-  UltrasonicSensor(Participant* participant,
+
-                   unsigned char pinTrigger,
+  UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
-                   unsigned char pinEcho);
 
   // parameters
 
   /// @brief The distance at which the object is considered to be touched
-  float touchDistance = 0.2f;
+  // float touchDistance = 0.2f;
 
   // state
 
@@ -31,15 +31,52 @@ class UltrasonicSensor : public TouchSensor {
   float GetDistance();
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(unsigned long currentTimeMs,
+  virtual void Update(bool recursive = false) override;
-                      bool recursive = false) override;
 
  protected:
   /// @brief The pin number of the trigger signal
   unsigned char pinTrigger = 0;
   /// @brief The pin number of the echo signal
   unsigned char pinEcho = 0;
+
+ public:
+  class DistanceSensor;
+  class TouchSensor;
 };
 
+#pragma region Distance sensor
+
+class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor {
+ public:
+  DistanceSensor(UltrasonicSensor::Configuration config,
+                 Thing* parent = Thing::LocalRoot());
+
+  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
+  virtual void Update(bool recursive = false) override;
+
+ protected:
+  UltrasonicSensor ultrasonic;
+};
+
+#pragma endregion Distance sensor
+
+#pragma region Touch sensor
+
+class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
+ public:
+  TouchSensor(UltrasonicSensor::Configuration config,
+              Thing* parent = Thing::LocalRoot());
+
+  float touchDistance = 0.2f;
+
+  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
+  virtual void Update(bool recursive = false) override;
+
+ protected:
+  UltrasonicSensor ultrasonic;
+};
+
+#pragma endregion Touch sensor
+
 }  // namespace Arduino
 }  // namespace RoboidControl

CMakeLists.txt

@@ -19,46 +19,27 @@ if(ESP_PLATFORM)
         REQUIRES esp_netif esp_wifi
     )
 else() 
-    project(RoboidControl)
-    add_subdirectory(LinearAlgebra)
-    add_subdirectory(Examples)
-
     set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-    add_compile_definitions(GTEST)
-    include(FetchContent)
-    FetchContent_Declare(
-        googletest
-        DOWNLOAD_EXTRACT_TIMESTAMP ON
-        URL https://github.com/google/googletest/archive/refs/heads/main.zip
-    )
-
-    # For Windows: Prevent overriding the parent project's compiler/linker settings
-    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
-    FetchContent_MakeAvailable(googletest)
-
     include_directories(
         .
         LinearAlgebra
     )
     add_library(RoboidControl STATIC ${srcs})
-    
+    target_include_directories(RoboidControl PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+    )
+
+    project(RoboidControl)
+    add_subdirectory(LinearAlgebra)
+
+    # Examples
+    option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF)
+    add_subdirectory(examples)
+
     enable_testing()
+    add_subdirectory(test)
 
-    file(GLOB_RECURSE test_srcs test/*_test.cc)
-    add_executable(
-        RoboidControlTest
-        ${test_srcs}
-    )
-    target_link_libraries(
-        RoboidControlTest
-        gtest_main
-        RoboidControl
-        LinearAlgebra
-    )
-
-    include(GoogleTest)
-    gtest_discover_tests(RoboidControlTest)
 endif()
 

DoxyGen/Doxyfile

@@ -48,7 +48,7 @@ PROJECT_NAME           = "Roboid Control for C++"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         =
+PROJECT_NUMBER         = 0.4
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

Examples/CMakeLists.txt

@@ -1,25 +0,0 @@
-# examples/CMakeLists.txt
-
-# Specify the minimum CMake version
-cmake_minimum_required(VERSION 3.10)
-
-# Specify the path to the main project directory
-set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..")
-
-# Set the project name
-project(Examples)
-
-include_directories(..)
-
-# Add the executable for the main project
-#add_executable(MainExecutable ${SOURCES})
-# Find the main project library (assuming it's defined in the root CMakeLists.txt)
-#find_package(RoboidControl REQUIRED)  # Replace MyLibrary with your actual library name
-
-# Add example executables
-add_executable(BB2B BB2B.cpp)
-target_link_libraries(
-    BB2B
-    RoboidControl
-    LinearAlgebra
-)

LinearAlgebra/Direction.cpp

@@ -9,6 +9,7 @@
 
 #include <math.h>
 
+namespace LinearAlgebra {
 template <typename T>
 DirectionOf<T>::DirectionOf() {
   this->horizontal = AngleOf<T>();
@@ -98,5 +99,6 @@ void DirectionOf<T>::Normalize() {
   }
 }
 
-template class DirectionOf<float>;
+template class LinearAlgebra::DirectionOf<float>;
-template class DirectionOf<signed short>;
+template class LinearAlgebra::DirectionOf<signed short>;
+}

LinearAlgebra/Direction.h

@@ -99,6 +99,4 @@ using Direction = DirectionSingle;
 
 }  // namespace LinearAlgebra
 
-using namespace LinearAlgebra;
-
 #endif

LinearAlgebra/Matrix.cpp

@@ -1,5 +1,7 @@
 #include "Matrix.h"
+#if !defined(NO_STD) 
 #include <iostream>
+#endif
 
 namespace LinearAlgebra {
 
@@ -61,7 +63,9 @@ Matrix2::Matrix2(const Matrix2& m)
     this->data = nullptr;
   else {
     this->data = new float[this->nValues];
-    std::copy(m.data, m.data + nValues, this->data);
+
+    for (int ix = 0; ix < this->nValues; ++ix)
+      this->data[ix] = m.data[ix];
   }
 }
 
@@ -76,7 +80,8 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
       this->data = nullptr;
     else {
       this->data = new float[this->nValues];
-      std::copy(m.data, m.data + this->nValues, this->data);
+      for (int ix = 0; ix < this->nValues; ++ix)
+        this->data[ix] = m.data[ix];
     }
   }
   return *this;
@@ -89,7 +94,8 @@ Matrix2::~Matrix2() {
 
 Matrix2 Matrix2::Clone() const {
   Matrix2 r = Matrix2(this->nRows, this->nCols);
-  std::copy(this->data, this->data + this->nValues, r.data);
+  for (int ix = 0; ix < this->nValues; ++ix)
+    r.data[ix] = this->data[ix];
   return r;
 }
 
@@ -133,7 +139,7 @@ Matrix2 Matrix2::Identity(int size) {
 }
 
 Matrix2 Matrix2::Diagonal(float f, int size) {
-  Matrix2 r = Matrix2(size, size);
+  Matrix2 r = Matrix2::Zero(size, size);
   float* data = r.data;
   int valueIx = 0;
   for (int ix = 0; ix < size; ix++) {
@@ -158,8 +164,8 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
 Matrix2 Matrix2::Transpose() const {
   Matrix2 r = Matrix2(this->nCols, this->nRows);
 
-  for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
+  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
-    for (uint colIx = 0; colIx < this->nCols; colIx++)
+    for (int colIx = 0; colIx < this->nCols; colIx++)
       r.data[colIx * this->nCols + rowIx] =
           this->data[rowIx * this->nCols + colIx];
   }
@@ -200,16 +206,16 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
     int BColOffset = i * BCols;  // BColOffset is constant for each row of B
     for (int j = 0; j < BCols; ++j) {
       float sum = 0;
-      // std::cout << " 0";
+      std::cout << " 0";
       int BIndex = j;
       for (int k = 0; k < ACols; ++k) {
-        // std::cout << " + " << this->data[ARowOffset + k] << " * "
+        std::cout << " + " << this->data[ARowOffset + k] << " * "
-        //           << B.data[BIndex];
+                  << B.data[BIndex];
         sum += this->data[ARowOffset + k] * B.data[BIndex];
         BIndex += BCols;
       }
       r.data[BColOffset + j] = sum;
-      // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
+      std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
     }
   }
   return r;

LinearAlgebra/Polar.cpp

@@ -175,5 +175,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
   return r;
 }
 
-template class PolarOf<float>;
+template class LinearAlgebra::PolarOf<float>;
-template class PolarOf<signed short>;
+template class LinearAlgebra::PolarOf<signed short>;

LinearAlgebra/Spherical.cpp

@@ -5,6 +5,8 @@
 
 #include <math.h>
 
+namespace LinearAlgebra {
+
 template <typename T>
 SphericalOf<T>::SphericalOf() {
   this->distance = 0.0f;
@@ -301,3 +303,5 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
 
 template class SphericalOf<float>;
 template class SphericalOf<signed short>;
+
+}  // namespace LinearAlgebra

LinearAlgebra/Spherical.h

@@ -186,7 +186,6 @@ using Spherical = SphericalSingle;
 #endif
 
 }  // namespace LinearAlgebra
-using namespace LinearAlgebra;
 
 #include "Polar.h"
 #include "Vector3.h"

LinearAlgebra/SwingTwist.cpp

@@ -4,6 +4,8 @@
 
 #include "SwingTwist.h"
 
+namespace LinearAlgebra {
+
 template <typename T>
 SwingTwistOf<T>::SwingTwistOf() {
   this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
@@ -165,4 +167,6 @@ void SwingTwistOf<T>::Normalize() {
 }
 
 template class SwingTwistOf<float>;
-template class SwingTwistOf<signed short>;
+template class SwingTwistOf<signed short>;
+
+}

LinearAlgebra/test/Direction_test.cc

@@ -6,6 +6,8 @@
 
 #include "Direction.h"
 
+using namespace LinearAlgebra;
+
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Direction16, Compare) {

LinearAlgebra/test/Polar_test.cc

@@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>
 
 #include "Polar.h"
 #include "Spherical.h"

LinearAlgebra/test/Spherical16_test.cc

@@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>
 
 #include "Spherical.h"
 #include "Vector3.h"

LinearAlgebra/test/SphericalSingle_test.cc

@@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>
 
 #include "Spherical.h"
 

Messages/BinaryMsg.cpp

@@ -6,7 +6,8 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thing = thing;
-  unsigned char ix = BinaryMsg::length;
+  unsigned char ix = 0; //BinaryMsg::length;
+  this->data = new char[255];
   this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }
 
@@ -40,6 +41,8 @@ unsigned char BinaryMsg::Serialize(char* buffer) {
   buffer[ix++] = this->networkId;
   buffer[ix++] = this->thingId;
   buffer[ix++] = this->dataLength;
+  for (int dataIx = 0; dataIx < this->dataLength; dataIx++)
+    buffer[ix++] = this->data[dataIx];
   return this->length + this->dataLength;
 }
 

Messages/BinaryMsg.h

@@ -1,15 +1,17 @@
 #pragma once
 
-#include "Messages.h"
+#include "IMessage.h"
+#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief Message to send thing-specific data
+/// @brief A message containing binary data for custom communication
 class BinaryMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0xB1;
-  /// @brief The length of the message without the binary data itslef
+  /// @brief The length of the message in bytes, excluding the binary data
+  /// For the total size of the message this.bytes.Length should be added to this value.
   static const unsigned length = 4;
 
   /// @brief The network ID of the thing
@@ -23,7 +25,7 @@ class BinaryMsg : public IMessage {
   /// @brief The binary data which is communicated
   char* data = nullptr;
 
-  /// @brief Create a new message for sending
+  /// @brief Create a BinaryMsg
   /// @param networkId The network ID of the thing
   /// @param thing The thing for which binary data is sent
   BinaryMsg(unsigned char networkId, Thing* thing);

Messages/DestroyMsg.cpp

@@ -2,19 +2,22 @@
 
 namespace RoboidControl {
 
-DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
 }
 
-DestroyMsg::DestroyMsg(char* buffer) {}
+DestroyMsg::DestroyMsg(char* buffer) {
+  this->networkId = buffer[1];
+  this->thingId = buffer[2];
+}
 
 DestroyMsg::~DestroyMsg() {}
 
-unsigned char DestroyMsg::Serialize(char *buffer) {
+unsigned char DestroyMsg::Serialize(char* buffer) {
-  #if defined(DEBUG)
+#if defined(DEBUG)
-  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId
+  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/"
-            << "] " << std::endl;
+            << (int)this->thingId << "] " << std::endl;
 #endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
@@ -23,4 +26,4 @@ unsigned char DestroyMsg::Serialize(char *buffer) {
   return ix;
 }
 
-} // namespace RoboidControl
+}  // namespace RoboidControl

Messages/DestroyMsg.h

@@ -1,13 +1,16 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief Message notifiying that a Thing no longer exists
+/// @brief A Message notifiying that a Thing no longer exists
 class DestroyMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0x20;
-  /// @brief The length of the message
+  /// @brief The length of the message in bytes
   static const unsigned length = 3;
   /// @brief The network ID of the thing
   unsigned char networkId;

Messages/IMessage.cpp

@@ -1,7 +1,4 @@
-#include "Messages.h"
+#include "IMessage.h"
-
-#include "LowLevelMessages.h"
-#include "string.h"
 
 namespace RoboidControl {
 

Messages/IMessage.h

@@ -0,0 +1,16 @@
+#pragma once
+
+namespace RoboidControl {
+
+/// @brief Root structure for all communcation messages
+class IMessage {
+ public:
+  IMessage();
+  /// @brief Serialize the message into a byte array for sending
+  /// @param buffer The buffer to serilize into
+  /// @return The length of the message in the buffer
+  virtual unsigned char Serialize(char* buffer);
+
+};
+
+}  // namespace RoboidControl

Messages/InvestigateMsg.cpp

@@ -7,9 +7,9 @@ InvestigateMsg::InvestigateMsg(char* buffer) {
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
 }
-InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
+InvestigateMsg::InvestigateMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
-  this->thingId = thingId;
+  this->thingId = thing->id;
 }
 
 InvestigateMsg::~InvestigateMsg() {}

Messages/InvestigateMsg.h

@@ -1,4 +1,7 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -14,10 +17,10 @@ class InvestigateMsg : public IMessage {
   /// @brief the ID of the thing
   unsigned char thingId;
 
-  /// @brief Create a new message for sending
+  /// @brief Create an investigate message
   /// @param networkId The network ID for the thing
-  /// @param thingId The ID of the thing
+  /// @param thing The thing for which the details are requested
-  InvestigateMsg(unsigned char networkId, unsigned char thingId);
+  InvestigateMsg(unsigned char networkId, Thing* thing);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   InvestigateMsg(char* buffer);
   /// @brief Destructor for the message

Messages/LowLevelMessages.cpp

@@ -5,6 +5,83 @@
 
 namespace RoboidControl {
 
+void LowLevelMessages::SendSpherical(char* buffer,
+                                     unsigned char* ix,
+                                     Spherical s) {
+  SendFloat16(buffer, ix, s.distance);
+  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
+  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
+}
+Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
+                                             unsigned char* startIndex) {
+  float distance = ReceiveFloat16(buffer, startIndex);
+
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle vertical = Angle::Radians(vertical8.InRadians());
+
+  Spherical s = Spherical(distance, horizontal, vertical);
+  return s;
+}
+
+void LowLevelMessages::SendSwingTwist(char* buffer,
+                                      unsigned char* ix,
+                                      SwingTwist s) {
+  SendAngle8(buffer, ix, s.swing.horizontal.InDegrees());
+  SendAngle8(buffer, ix, s.swing.vertical.InDegrees());
+  SendAngle8(buffer, ix, s.twist.InDegrees());
+}
+
+SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer,
+                                               unsigned char* startIndex) {
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle vertical = Angle::Radians(vertical8.InRadians());
+
+  Angle8 twist8 = ReceiveAngle8(buffer, startIndex);
+  Angle twist = Angle::Radians(twist8.InRadians());
+
+  SwingTwist s = SwingTwist(horizontal, vertical, twist);
+  return s;
+}
+
+void LowLevelMessages::SendQuat32(char* buffer,
+                                  unsigned char* ix,
+                                  SwingTwist rotation) {
+  Quaternion q = rotation.ToQuaternion();
+  unsigned char qx = (char)(q.x * 127 + 128);
+  unsigned char qy = (char)(q.y * 127 + 128);
+  unsigned char qz = (char)(q.z * 127 + 128);
+  unsigned char qw = (char)(q.w * 255);
+  if (q.w < 0) {
+    qx = -qx;
+    qy = -qy;
+    qz = -qz;
+    qw = -qw;
+  }
+  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
+  // << "\n";
+  buffer[(*ix)++] = qx;
+  buffer[(*ix)++] = qy;
+  buffer[(*ix)++] = qz;
+  buffer[(*ix)++] = qw;
+}
+
+SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
+                                           unsigned char* ix) {
+  float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qw = buffer[(*ix)++] / 255.0F;
+  Quaternion q = Quaternion(qx, qy, qz, qw);
+  SwingTwist s = SwingTwist::FromQuaternion(q);
+  return s;
+}
+
 void LowLevelMessages::SendAngle8(char* buffer,
                                   unsigned char* ix,
                                   const float angle) {
@@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
   return (float)f.toFloat();
 }
 
-void LowLevelMessages::SendSpherical(char* buffer,
-                                     unsigned char* ix,
-                                     Spherical s) {
-  SendFloat16(buffer, ix, s.distance);
-  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
-  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
-}
-Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
-                                             unsigned char* startIndex) {
-  float distance = ReceiveFloat16(buffer, startIndex);
-
-  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
-  Angle horizontal = Angle::Radians(horizontal8.InRadians());
-
-  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
-  Angle vertical = Angle::Radians(vertical8.InRadians());
-
-  Spherical s = Spherical(distance, horizontal, vertical);
-  return s;
-}
-
-void LowLevelMessages::SendQuat32(char* buffer,
-                                  unsigned char* ix,
-                                  SwingTwist rotation) {
-  Quaternion q = rotation.ToQuaternion();
-  unsigned char qx = (char)(q.x * 127 + 128);
-  unsigned char qy = (char)(q.y * 127 + 128);
-  unsigned char qz = (char)(q.z * 127 + 128);
-  unsigned char qw = (char)(q.w * 255);
-  if (q.w < 0) {
-    qx = -qx;
-    qy = -qy;
-    qz = -qz;
-    qw = -qw;
-  }
-  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
-  // << "\n";
-  buffer[(*ix)++] = qx;
-  buffer[(*ix)++] = qy;
-  buffer[(*ix)++] = qz;
-  buffer[(*ix)++] = qw;
-}
-
-SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
-                                           unsigned char* ix) {
-  float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qw = buffer[(*ix)++] / 255.0F;
-  Quaternion q = Quaternion(qx, qy, qz, qw);
-  SwingTwist s = SwingTwist::FromQuaternion(q);
-  return s;
-}
 
 }  // namespace RoboidControl

Messages/LowLevelMessages.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #include "LinearAlgebra/Spherical.h"
 #include "LinearAlgebra/SwingTwist.h"
 
@@ -5,18 +7,22 @@ namespace RoboidControl {
 
 class LowLevelMessages {
  public:
+  static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
+  static Spherical ReceiveSpherical(const char* buffer,
+                                    unsigned char* startIndex);
+
+  static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s);
+  static SwingTwist ReceiveSwingTwist(const char* buffer,
+                                      unsigned char* startIndex);
+
+  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
+  static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
+
   static void SendAngle8(char* buffer, unsigned char* ix, const float angle);
   static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex);
 
   static void SendFloat16(char* buffer, unsigned char* ix, float value);
   static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
-
-  static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
-  static Spherical ReceiveSpherical(const char* buffer,
-                                    unsigned char* startIndex);
-
-  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
-  static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
 };
 
 }  // namespace RoboidControl

Messages/Messages.h

@@ -1,22 +0,0 @@
-#pragma once
-
-#include "LinearAlgebra/Spherical.h"
-#include "LinearAlgebra/SwingTwist.h"
-#include "Thing.h"
-
-namespace RoboidControl {
-
-class ParticipantUDP;
-
-class IMessage {
- public:
-  IMessage();
-  virtual unsigned char Serialize(char* buffer);
-
-  static unsigned char* ReceiveMsg(unsigned char packetSize);
-
-  // bool Publish(ParticipantUDP *participant);
-  // bool SendTo(ParticipantUDP *participant);
-};
-
-}  // namespace RoboidControl

Messages/ModelUrlMsg.h

@@ -1,4 +1,7 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -26,8 +29,6 @@ class ModelUrlMsg : public IMessage {
   ModelUrlMsg(unsigned char networkId, Thing* thing);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   ModelUrlMsg(const char* buffer);
-  //   ModelUrlMsg(unsigned char networkId, unsigned char thingId,
-  //               unsigned char urlLegth, const char *url, float scale = 1);
 
   /// @brief Destructor for the message
   virtual ~ModelUrlMsg();

Messages/NameMsg.cpp

@@ -7,15 +7,16 @@ namespace RoboidControl {
 NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
-  if (thing->name == nullptr)
+  const char* thingName = thing->GetName();
+  if (thingName == nullptr)
     this->nameLength = 0;
   else
-    this->nameLength = (unsigned char)strlen(thing->name);
+    this->nameLength = (unsigned char)strlen(thingName);
 
   // the name string in the buffer is not \0 terminated!
   char* name = new char[this->nameLength + 1];
   for (int i = 0; i < this->nameLength; i++)
-    name[i] = thing->name[i];
+    name[i] = thingName[i];
   name[this->nameLength] = '\0';
   this->name = name;
 }

Messages/NameMsg.h

@@ -1,4 +1,7 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -22,9 +25,6 @@ class NameMsg : public IMessage {
   /// @param networkId The network ID of the thing
   /// @param thing The ID of the thing
   NameMsg(unsigned char networkId, Thing* thing);
-  //   NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
-  //           unsigned char nameLength);
-
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   NameMsg(const char* buffer);
   /// @brief Destructor for the message

Messages/NetworkIdMsg.cpp

@@ -0,0 +1,27 @@
+#include "NetworkIdMsg.h"
+
+#include <iostream>
+
+namespace RoboidControl {
+
+NetworkIdMsg::NetworkIdMsg(const char* buffer) {
+  this->networkId = buffer[1];
+}
+
+NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
+  this->networkId = networkId;
+}
+
+NetworkIdMsg::~NetworkIdMsg() {}
+
+unsigned char NetworkIdMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl;
+#endif
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return NetworkIdMsg::length;
+}
+
+}  // namespace RoboidControl

Messages/NetworkIdMsg.h

@@ -1,9 +1,11 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
 
 namespace RoboidControl {
 
 /// @brief A message communicating the network ID for that participant
-class SiteMsg : public IMessage {
+class NetworkIdMsg : public IMessage {
 public:
   /// @brief The message ID
   static const unsigned char id = 0xA1;
@@ -14,11 +16,11 @@ public:
 
   /// @brief Create a new message for sending
   /// @param networkId The network ID for the participant
-  SiteMsg(unsigned char networkId);
+  NetworkIdMsg(unsigned char networkId);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
-  SiteMsg(const char *buffer);
+  NetworkIdMsg(const char *buffer);
   /// @brief Destructor for the message
-  virtual ~SiteMsg();
+  virtual ~NetworkIdMsg();
 
   /// @copydoc RoboidControl::IMessage::Serialize
   virtual unsigned char Serialize(char *buffer) override;

Messages/ParticipantMsg.cpp

@@ -1,5 +1,9 @@
 #include "ParticipantMsg.h"
 
+#if !defined(NO_STD)
+#include <iostream>
+#endif
+
 namespace RoboidControl {
 
 ParticipantMsg::ParticipantMsg(char networkId) {
@@ -13,7 +17,7 @@ ParticipantMsg::ParticipantMsg(const char* buffer) {
 ParticipantMsg::~ParticipantMsg() {}
 
 unsigned char ParticipantMsg::Serialize(char* buffer) {
-#if defined(DEBUG)
+#if defined(DEBUG) && !defined(NO_STD)
   std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] "
             << std::endl;
 #endif

Messages/ParticipantMsg.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "Messages.h"
+#include "IMessage.h"
 
 namespace RoboidControl {
 

Messages/PoseMsg.cpp

@@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
   this->thingId = thing->id;
 
   this->poseType = 0;
-  if (thing->positionUpdated || force) {
+  if (thing->positionUpdated || (force || thing->IsRoot())) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
   }
-  if (thing->orientationUpdated || force) {
+  if (thing->orientationUpdated || (force || thing->IsRoot())) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
   }
@@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
   this->thingId = buffer[ix++];
   this->poseType = buffer[ix++];
   this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
-  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
   // linearVelocity
   // angularVelocity
 }
@@ -45,7 +45,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if (this->poseType == 0)
     return 0;
 
-#if defined(DEBUG)
+#if defined(DEBUG) && DEBUG > 1
   std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
             << (int)this->thingId << "] " << (int)this->poseType << std::endl;
 #endif
@@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if ((this->poseType & Pose_Position) != 0)
     LowLevelMessages::SendSpherical(buffer, &ix, this->position);
   if ((this->poseType & Pose_Orientation) != 0)
-    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
+    LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
   if ((this->poseType & Pose_LinearVelocity) != 0)
     LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
   if ((this->poseType & Pose_AngularVelocity) != 0)

Messages/PoseMsg.h

@@ -1,4 +1,6 @@
-#include "Messages.h"
+#pragma once
+#include "IMessage.h"
+#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -9,7 +11,7 @@ class PoseMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0x10;
-  /// @brief The length of the message
+  /// @brief The length of the message in bytes
   unsigned char length = 4 + 4 + 4;
 
   /// @brief The network ID of the thing
@@ -40,7 +42,8 @@ class PoseMsg : public IMessage {
 
   /// @brief Create a new message for sending
   /// @param networkId he network ID of the thing
-  /// @param thing The thing for which the pose shouldbe sent
+  /// @param thing The thing for which the pose should be sent
+  /// @param force If true, position and orientation are always included, even when they are not updated
   PoseMsg(unsigned char networkId, Thing* thing, bool force = false);
 
   /// @copydoc RoboidControl::IMessage::IMessage(char*)

Messages/SiteMsg.cpp

@@ -1,25 +0,0 @@
-#include "SiteMsg.h"
-
-namespace RoboidControl {
-
-SiteMsg::SiteMsg(const char* buffer) {
-  this->networkId = buffer[1];
-}
-
-SiteMsg::SiteMsg(unsigned char networkId) {
-  this->networkId = networkId;
-}
-
-SiteMsg::~SiteMsg() {}
-
-unsigned char SiteMsg::Serialize(char* buffer) {
-#if defined(DEBUG)
-  std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl;
-#endif
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  return SiteMsg::length;
-}
-
-}  // namespace RoboidControl

Messages/TextMsg.cpp

@@ -1,5 +1,9 @@
 #include "TextMsg.h"
 
+#if !defined(NO_STD)
+#include <iostream>
+#endif
+
 namespace RoboidControl {
 
 TextMsg::TextMsg(const char* text, unsigned char textLength) {
@@ -24,7 +28,7 @@ unsigned char TextMsg::Serialize(char* buffer) {
   if (this->textLength == 0 || this->text == nullptr)
     return 0;
 
-#if defined(DEBUG)
+#if defined(DEBUG) && !defined(NO_STD)
   std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl;
 #endif
   unsigned char ix = 0;

Messages/TextMsg.h

@@ -1,4 +1,4 @@
-#include "Messages.h"
+#include "IMessage.h"
 
 namespace RoboidControl {
 
@@ -9,10 +9,6 @@ class TextMsg : public IMessage {
   static const unsigned char id = 0xB0;
   /// @brief The length of the message without the text itself
   static const unsigned char length = 2;
-  /// @brief The network ID of the thing
-  unsigned char networkId;
-  /// @brief the ID of the thing
-  unsigned char thingId;
   /// @brief The text without the null terminator
   const char* text;
   /// @brief The length of the text

Messages/ThingMsg.cpp

@@ -14,27 +14,21 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thingType = thing->type;
-  Thing* parent = thing->GetParent();
+  if (thing->IsRoot())
-  if (parent != nullptr)
-    this->parentId = parent->id;
-  else
     this->parentId = 0;
+  else {
+    Thing* parent = thing->GetParent();
+    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) {
 #if defined(DEBUG)
-  std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId
+  std::cout << "Send ThingMsg [" << (int)this->networkId << "/"
-            << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl;
+            << (int)this->thingId << "] " << (int)this->thingType << " "
+            << (int)this->parentId << std::endl;
 #endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;

Messages/ThingMsg.h

@@ -1,8 +1,9 @@
-#include "Messages.h"
+#include "IMessage.h"
+#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief  Message providing generic information about a Thing
+/// @brief Message providing generic details about a Thing
 class ThingMsg : public IMessage {
  public:
   /// @brief The message ID
@@ -13,17 +14,15 @@ class ThingMsg : public IMessage {
   unsigned char networkId;
   /// @brief The ID of the thing
   unsigned char thingId;
-  /// @brief The Thing.Type of the thing
+  /// @brief The type of thing
   unsigned char thingType;
-  /// @brief The parent of the thing in the hierarachy. This is null for root Things
+  /// @brief The ID of the parent thing in the hierarchy. This is zero for root things
   unsigned char parentId;
 
   /// @brief Create a message for sending
   /// @param networkId The network ID of the thing</param>
   /// @param thing The thing
   ThingMsg(unsigned char networkId, Thing* thing);
-  //   ThingMsg(unsigned char networkId, unsigned char thingId,
-  //            unsigned char thingType, unsigned char parentId);
 
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   ThingMsg(const char* buffer);

Participant.cpp

@@ -1,12 +1,33 @@
 #include "Participant.h"
 
 #include <string.h>
+#include "Arduino/ArduinoParticipant.h"
+#include "EspIdf/EspIdfParticipant.h"
+#include "Posix/PosixParticipant.h"
+#include "Windows/WindowsParticipant.h"
 
 namespace RoboidControl {
 
-Participant::Participant() {}
+#pragma region Participant
+
+ParticipantRegistry Participant::registry;
+
+Participant* Participant::LocalParticipant = new Participant();
+
+void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
+  LocalParticipant = &newParticipant;
+  std::cout << "Replaced local participant" << std::endl;
+}
+
+Participant::Participant() {
+  Thing::CreateRoot(this);
+  //this->Add(this->root);
+}
 
 Participant::Participant(const char* ipAddress, int port) {
+  Thing::CreateRoot(this);
+  //this->Add(this->root);
+
   // make a copy of the ip address string
   int addressLength = (int)strlen(ipAddress);
   int stringLength = addressLength + 1;
@@ -24,23 +45,52 @@ Participant::Participant(const char* ipAddress, int port) {
 }
 
 Participant::~Participant() {
+  // registry.Remove(this);
   delete[] this->ipAddress;
 }
 
-void Participant::Update(unsigned long currentTimeMs) {
+void Participant::Update() {
   for (Thing* thing : this->things) {
     if (thing != nullptr)
-      thing->Update(currentTimeMs, true);
+      thing->Update(true);
   }
 }
 
+bool Participant::Send(IMessage* msg) {
+  int bufferSize = msg->Serialize(this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
+  //           << " to " << this->ipAddress << std::endl;
+
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  return thisWindows->Send(this, bufferSize);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  return thisPosix->Send(this, bufferSize);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  return thisArduino->Send(this, bufferSize);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  return thisEspIdf->Send(remoteParticipant, bufferSize);
+#else
+  return false;
+#endif
+}
+
 Thing* Participant::Get(unsigned char thingId) {
   for (Thing* thing : this->things) {
     if (thing->id == thingId)
       return thing;
   }
-  // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
+  std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
-  //           << "[" << (int)thingId << "]\n";
+            << "[" << (int)thingId << "]\n";
   return nullptr;
 }
 
@@ -51,7 +101,15 @@ void Participant::Add(Thing* thing, bool checkId) {
     thing->id = this->thingCount + 1;
     this->things[this->thingCount++] = thing;
 #else
-    thing->id = (unsigned char)this->things.size() + 1;
+    // find highest id
+    int highestIx = 0;
+    for (Thing* thing : this->things) {
+      if (thing == nullptr)
+        continue;
+      if (thing->id > highestIx)
+        highestIx = thing->id;
+    }
+    thing->id = highestIx + 1;
     this->things.push_back(thing);
 #endif
     // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
@@ -64,7 +122,8 @@ void Participant::Add(Thing* thing, bool checkId) {
 #else
       this->things.push_back(thing);
 #endif
-      // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "["
+      // std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
+      // "["
       //           << (int)thing->id << "]\n";
     } else {
       // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
@@ -89,22 +148,93 @@ void Participant::Remove(Thing* thing) {
   this->thingCount = lastThingIx;
 #else
   this->things.remove_if([thing](Thing* obj) { return obj == thing; });
-  std::cout << "Removing " << thing->networkId << "/" << thing->id
+  // std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
-            << " list size = " << this->things.size() << "\n";
+  //           << "] list size = " << this->things.size() << "\n";
 #endif
 }
 
-// void Participant::UpdateAll(unsigned long currentTimeMs) {
+#pragma endregion
-//   // Not very efficient, but it works for now.
 
-//   for (Thing* thing : this->things) {
+#pragma region ParticipantRegistry
-//     if (thing != nullptr && thing->GetParent() == nullptr) {  // update all
+
-//     root things
+Participant* ParticipantRegistry::Get(const char* ipAddress,
-//       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
+                                      unsigned int port) {
-//       //           << "\n";
+#if !defined(NO_STD)
-//       thing->Update(currentTimeMs);
+  for (Participant* participant : ParticipantRegistry::participants) {
-//     }
+    if (participant == nullptr)
-//   }
+      continue;
-// }
+    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
+        participant->port == port) {
+      // std::cout << "found participant " << participant->ipAddress << ":"
+      //           << (int)participant->port << std::endl;
+      return participant;
+    }
+  }
+  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
+            << std::endl;
+#endif
+  return nullptr;
+}
+
+Participant* ParticipantRegistry::Get(unsigned char participantId) {
+#if !defined(NO_STD)
+  for (Participant* participant : ParticipantRegistry::participants) {
+    if (participant == nullptr)
+      continue;
+    if (participant->networkId == participantId)
+      return participant;
+  }
+  std::cout << "Could not find participant " << (int)participantId << std::endl;
+#endif
+  return nullptr;
+}
+
+Participant* ParticipantRegistry::Add(const char* ipAddress,
+                                      unsigned int port) {
+  Participant* participant = new Participant(ipAddress, port);
+  Add(participant);
+  return participant;
+}
+
+void ParticipantRegistry::Add(Participant* participant) {
+  Participant* foundParticipant =
+      Get(participant->ipAddress, participant->port);
+
+  if (foundParticipant == nullptr) {
+#if defined(NO_STD)
+    // this->things[this->thingCount++] = thing;
+#else
+    ParticipantRegistry::participants.push_back(participant);
+#endif
+    //   std::cout << "Add participant " << participant->ipAddress << ":"
+    //             << participant->port << "[" << (int)participant->networkId
+    //             << "]\n";
+    //   std::cout << "participants " <<
+    //   ParticipantRegistry::participants.size()
+    //             << "\n";
+    // } else {
+    //   std::cout << "Did not add, existing participant " <<
+    //   participant->ipAddress
+    //             << ":" << participant->port << "[" <<
+    //             (int)participant->networkId
+    //             << "]\n";
+  }
+}
+
+void ParticipantRegistry::Remove(Participant* participant) {
+  // participants.remove(participant);
+}
+
+#if defined(NO_STD)
+Participant** ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#else
+const std::list<Participant*>& ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#endif
+
+#pragma endregion ParticipantRegistry
 
 }  // namespace RoboidControl

Participant.h

@@ -1,10 +1,58 @@
 #pragma once
+
+#include "Messages/IMessage.h"
 #include "Thing.h"
 
 namespace RoboidControl {
 
 constexpr int MAX_THING_COUNT = 256;
 
+/// @brief class which manages all known participants
+class ParticipantRegistry {
+ public:
+  /// @brief Retrieve a participant by its address
+  /// @param ipAddress The IP address of the participant
+  /// @param port The port number of the participant
+  /// @return The participant or a nullptr when it could not be found
+  Participant* Get(const char* ipAddress, unsigned int port);
+  /// @brief Retrieve a participant by its network ID
+  /// @param networkID The network ID of the participant
+  /// @return The participant or a nullptr when it could not be found
+  Participant* Get(unsigned char networkID);
+
+  /// @brief Add a participant with the given details
+  /// @param ipAddress The IP address of the participant
+  /// @param port The port number of the participant
+  /// @return The added participant
+  Participant* Add(const char* ipAddress, unsigned int port);
+  /// @brief Add a participant
+  /// @param participant The participant to add
+  void Add(Participant* participant);
+
+  /// @brief Remove a participant
+  /// @param participant The participant to remove
+  void Remove(Participant* participant);
+
+ private:
+#if defined(NO_STD)
+ public:
+  Participant** GetAll() const;
+  int count = 0;
+
+ private:
+  Participant** participants;
+#else
+ public:
+  /// @brief Get all participants
+  /// @return All participants
+  const std::list<Participant*>& GetAll() const;
+
+ private:
+  /// @brief The list of known participants
+  std::list<Participant*> participants;
+#endif
+};
+
 /// @brief A participant is a device which manages things.
 /// It can communicate with other participant to synchronise the state of
 /// things. This class is used to register the things the participant is
@@ -12,53 +60,94 @@ constexpr int MAX_THING_COUNT = 256;
 /// participant. It is used as a basis for the local participant, but also as a
 /// reference to remote participants.
 class Participant {
+#pragma region Init
+
  public:
-  /// @brief The Ip Address of a participant. When the participant is local,
+  /// @brief Create a generic participant
-  /// this contains 0.0.0.0
-  const char* ipAddress = "0.0.0.0";
-  /// @brief The port number for UDP communication with the participant. This is
-  /// 0 for isolated participants.
-  int port = 0;
-
-  /// @brief The network Id to identify the participant.
-  /// @note This field is likely to disappear in future versions
-  unsigned char networkId = 0;
-
-  /// @brief Default constructor
   Participant();
   /// @brief Create a new participant with the given communcation info
   /// @param ipAddress The IP address of the participant
-  /// @param port The port of the participant
+  /// @param port The UDP port of the participant
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
   Participant(const char* ipAddress, int port);
   /// @brief Destructor for the participant
   ~Participant();
 
-  virtual void Update(unsigned long currentTimeMs = 0);
+  /// @brief The local participant for this application
+  static Participant* LocalParticipant;
+  /// @brief Replace the local participant
+  /// @param newParticipant The new local Participant
+  static void ReplaceLocalParticipant(Participant& newParticipant);
 
- protected:
+#pragma endregion Init
+
+#pragma region Properties
+
+ public:
+  /// @brief The name of the participant
+  const char* name = "Participant";
+
+  /// @brief The Ip Address of a participant.
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  const char* ipAddress = "0.0.0.0";
+  /// @brief The port number for UDP communication with the participant.
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  unsigned int port = 0;
+
+  /// @brief The network Id to identify the participant
+  unsigned char networkId = 0;
+
+  /// @brief The root thing for this participant
+  Thing* root = nullptr;
+
+ public:
 #if defined(NO_STD)
   unsigned char thingCount = 0;
   Thing* things[MAX_THING_COUNT];
 #else
-  /// @brief The list of things managed by this participant
+  /// @brief  The things managed by this participant
   std::list<Thing*> things;
 #endif
-
- public:
   /// @brief Find a thing managed by this participant
-  /// @param networkId The network ID for the thing
   /// @param thingId The ID of the thing
-  /// @return The thing if found or nullptr when no thing has been found
+  /// @return The thing if found, nullptr when no thing has been found
-  /// @note The use of the network ID is likely to disappear in future versions.
   Thing* Get(unsigned char thingId);
   /// @brief Add a new thing for this participant.
   /// @param thing The thing to add
-  /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
+  /// @param checkId If true, the thing.id is regenerated if it is zero
-  /// Id will be assigned.
   void Add(Thing* thing, bool checkId = true);
   /// @brief Remove a thing for this participant
   /// @param thing The thing to remove
   void Remove(Thing* thing);
+
+#pragma endregion Properties
+
+#pragma region Update
+
+ public:
+  /// @brief Update all things for this participant
+  virtual void Update();
+
+#pragma endregion Update
+
+#pragma region Send
+
+ public:
+  char buffer[1024];
+
+  virtual bool Send(IMessage* msg);
+
+#pragma endregion Send
+
+#pragma region Participant Registry
+
+ public:
+  static ParticipantRegistry registry;
+
+#pragma endregion Participant Registry
 };
 
 }  // namespace RoboidControl

Participants/ParticipantUDP.cpp

@@ -1,56 +1,53 @@
 #include "ParticipantUDP.h"
 
+#include "Participant.h"
 #include "Thing.h"
 
 #include "Arduino/ArduinoParticipant.h"
 #include "EspIdf/EspIdfParticipant.h"
-
-#if defined(_WIN32) || defined(_WIN64)
-#include <winsock2.h>
-#include <ws2tcpip.h>
-#include "Windows/WindowsParticipant.h"
-#pragma comment(lib, "ws2_32.lib")
-
-#elif defined(__unix__) || defined(__APPLE__)
-#include <arpa/inet.h>
-#include <fcntl.h>  // For fcntl
-#include <netinet/in.h>
-#include <sys/socket.h>
-#include <unistd.h>
-#include <chrono>
 #include "Posix/PosixParticipant.h"
-#endif
+#include "Windows/WindowsParticipant.h"
 
 #include <string.h>
 
 namespace RoboidControl {
 
-ParticipantUDP::ParticipantUDP(int port) {
+#pragma region Init
-  this->ipAddress = "0.0.0.0";
+
-  this->port = port;
+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);
 
-static ParticipantUDP* isolatedParticipant = nullptr;
+  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  this->Add(this->root);
 
-ParticipantUDP* ParticipantUDP::Isolated() {
+  Participant::ReplaceLocalParticipant(*this);
-  if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDP(0);
-  return isolatedParticipant;
 }
 
 void ParticipantUDP::begin() {
-  if (this->isIsolated)
+  if (this->isIsolated || this->remoteSite == nullptr)
     return;
 
   SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
@@ -69,7 +66,7 @@ void ParticipantUDP::SetupUDP(int localPort,
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
       static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+  thisArduino->Setup();
 #elif defined(IDF_VER)
   EspIdf::ParticipantUDP* thisEspIdf =
       static_cast<EspIdf::ParticipantUDP*>(this);
@@ -78,9 +75,16 @@ void ParticipantUDP::SetupUDP(int localPort,
   this->connected = true;
 }
 
-void ParticipantUDP::Update(unsigned long currentTimeMs) {
+#pragma endregion Init
-  if (currentTimeMs == 0)
+
-    currentTimeMs = Thing::GetTimeMs();
+#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();
 
   if (this->isIsolated == false) {
     if (this->connected == false)
@@ -91,7 +95,8 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
       if (this->remoteSite == nullptr)
         this->Publish(msg);
       else
-        this->Send(this->remoteSite, msg);
+        this->Send(msg);
+
       delete msg;
 
       this->nextPublishMe = currentTimeMs + this->publishInterval;
@@ -100,107 +105,85 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
     this->ReceiveUDP();
   }
 
+  UpdateMyThings();
+  UpdateOtherThings();
+}
+
+void ParticipantUDP::UpdateMyThings() {
   for (Thing* thing : this->things) {
-    if (thing == nullptr)
+    if (thing == nullptr)  // || thing->GetParent() != nullptr)
       continue;
 
-    std::cout << "Update thing " << (int)thing->id << std::endl;
+    // Why don't we do recursive?
-    if (this->isIsolated == false) {
+    // Because when a thing creates a thing in the update,
-      PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+    // that new thing is not sent out (because of hierarchyChanged)
-      this->Send(thing->owner, poseMsg);
+    // before it is updated itself: it is immediatedly updated!
-      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+    thing->Update(false);
-      this->Send(thing->owner, binaryMsg);
+
-      delete poseMsg;
+    if (thing->terminate)
-    }
+      this->Remove(thing);
-    thing->Update(currentTimeMs, true);
   }
 }
 
-void ParticipantUDP::ReceiveUDP() {
+void ParticipantUDP::UpdateOtherThings() {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  thisWindows->Receive();
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  thisPosix->Receive();
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Receive();
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  thisEspIdf->Receive();
-#endif
-}
-
-Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) {
-  for (Participant* sender : this->senders) {
-    if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
-      return sender;
-  }
-  return nullptr;
-}
-
-Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
-  // std::cout << "New Participant " << ipAddress << ":" << port << "\n";
-  Participant* participant = new Participant(ipAddress, port);
 #if defined(NO_STD)
-  participant->networkId = this->senderCount;
+  Participant** participants = Participant::registry.GetAll();
-  this->senders[this->senderCount++] = participant;
+  for (int ix = 0; ix < Participant::registry.count; ix++) {
+    Participant* participant = participants[ix];
 #else
-  participant->networkId = (unsigned char)this->senders.size();
+  for (Participant* participant : Participant::registry.GetAll()) {
-  this->senders.push_back(participant);
 #endif
-  return participant;
+    if (participant == nullptr || participant == this)
+      continue;
+
+    // Call only the Participant version of the Update.
+    // This is to deal with the function where one of the (remote)
+    // participants is actually a local participant
+    participant->Participant::Update();
+    if (this->isIsolated)
+      continue;
+
+    for (Thing* thing : participant->things) {
+      PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
+      participant->Send(poseMsg);
+      delete poseMsg;
+      BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
+      participant->Send(binaryMsg);
+      delete binaryMsg;
+    }
+  }
 }
 
+// Update
+#pragma endregion
+
 #pragma region Send
 
 void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
                                    Thing* thing) {
   // std::cout << "Send thing info [" << (int)thing->id << "] \n";
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-  this->Send(remoteParticipant, thingMsg);
+  remoteParticipant->Send(thingMsg);
   delete thingMsg;
   NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  this->Send(remoteParticipant, nameMsg);
+  remoteParticipant->Send(nameMsg);
   delete nameMsg;
   ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Send(remoteParticipant, modelMsg);
+  remoteParticipant->Send(modelMsg);
   delete modelMsg;
   PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  this->Send(remoteParticipant, poseMsg);
+  remoteParticipant->Send(poseMsg);
   delete poseMsg;
-  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
+  BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-  this->Send(remoteParticipant, customMsg);
+  remoteParticipant->Send(binaryMsg);
-  delete customMsg;
+  delete binaryMsg;
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
+bool ParticipantUDP::Send(IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
+  if (this->remoteSite != nullptr)
-  if (bufferSize <= 0)
+    return this->remoteSite->Send(msg);
-    return true;
+    
-
+  return true;
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  return thisWindows->Send(remoteParticipant, bufferSize);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  return thisPosix->Send(remoteParticipant, bufferSize);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  return thisArduino->Send(remoteParticipant, bufferSize);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->Send(remoteParticipant, bufferSize);
-#else
-  return false;
-#endif
 }
 
 void ParticipantUDP::PublishThingInfo(Thing* thing) {
@@ -225,6 +208,7 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) {
 }
 
 bool ParticipantUDP::Publish(IMessage* msg) {
+  // std::cout << "publish msg\n";
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
       static_cast<Windows::ParticipantUDP*>(this);
@@ -250,14 +234,37 @@ bool ParticipantUDP::Publish(IMessage* msg) {
 
 #pragma region Receive
 
+void ParticipantUDP::ReceiveUDP() {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  thisWindows->Receive();
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  thisPosix->Receive();
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  thisArduino->Receive();
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  thisEspIdf->Receive();
+#endif
+}
+
 void ParticipantUDP::ReceiveData(unsigned char packetSize,
                                  char* senderIpAddress,
                                  unsigned int senderPort) {
-  Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
+  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
+  //           << std::endl;
+  Participant* sender = this->registry.Get(senderIpAddress, senderPort);
   if (sender == nullptr) {
-    sender = this->AddParticipant(senderIpAddress, senderPort);
+    sender = this->registry.Add(senderIpAddress, senderPort);
+#if !defined(NO_STD)
     std::cout << "New remote participant " << sender->ipAddress << ":"
               << sender->port << std::endl;
+#endif
   }
 
   ReceiveData(packetSize, sender);
@@ -275,8 +282,8 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
       Process(sender, msg);
       delete msg;
     } break;
-    case SiteMsg::id: {
+    case NetworkIdMsg::id: {
-      SiteMsg* msg = new SiteMsg(this->buffer);
+      NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
       bufferSize -= msg->length;
       Process(sender, msg);
       delete msg;
@@ -316,11 +323,25 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
       Process(sender, msg);
       delete msg;
     } break;
+    case TextMsg::id: {
+      TextMsg* msg = new TextMsg(this->buffer);
+      bufferSize -= msg->length + msg->textLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case DestroyMsg::id: {
+      DestroyMsg* msg = new DestroyMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
   };
 
-  // Check if the buffer has been read completely
+// Check if the buffer has been read completely
+#if !defined(NO_STD)
   if (bufferSize > 0)
     std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
+#endif
 }
 
 void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
@@ -330,15 +351,16 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
 #endif
 }
 
-void ParticipantUDP::Process(Participant* sender, SiteMsg* 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);
   }
@@ -380,11 +402,15 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
             nameLength);  // Leave space for null terminator
 #endif
     thingName[nameLength] = '\0';
-    thing->name = thingName;
+    thing->SetName(thingName);
 
-    std::cout << thing->name;
+#if !defined(NO_STD)
+    std::cout << thing->GetName();
+#endif
   }
+#if !defined(NO_STD)
   std::cout << std::endl;
+#endif
 }
 
 void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
@@ -395,26 +421,66 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
 }
 
 void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
-#if defined(DEBUG)
+#if !defined(DEBUG) && !defined(NO_STD)
   std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
-            << "/" << (int)msg->networkId << "]\n";
+            << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
 #endif
+  Participant* owner = Participant::registry.Get(msg->networkId);
+  if (owner == nullptr)
+    return;
+
+  Thing* thing = owner->Get(msg->thingId);
+  if (thing == nullptr)
+    return;
+
+  if ((msg->poseType & PoseMsg::Pose_Position) != 0)
+    thing->SetPosition(msg->position);
+  if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
+    thing->SetOrientation(msg->orientation);
+  if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
+    thing->SetLinearVelocity(msg->linearVelocity);
+  if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
+    thing->SetAngularVelocity(msg->angularVelocity);
 }
 
 void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "] ";
+            << "/" << (int)msg->thingId << "]\n";
+#endif
+
+  Participant* owner = Participant::registry.Get(msg->networkId);
+  if (owner != nullptr) {
+    Thing* thing = owner->Get(msg->thingId);
+    if (thing != nullptr)
+      thing->ProcessBinary(msg->data);
+#if !defined(NO_STD)
+    else {
+#if defined(DEBUG)
+      std::cout << "  unknown thing [" << (int)msg->networkId << "/"
+                << (int)msg->thingId << "]";
+#endif
+    }
+#endif
+  }
+}
+
+void ParticipantUDP::Process(Participant* sender, TextMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
+            << (int)msg->text << "\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
+            << (int)msg->thingId << "]\n";
 #endif
 
   Thing* thing = sender->Get(msg->thingId);
   if (thing != nullptr)
-    thing->ProcessBinary(msg->data);
+    this->Remove(thing);
-  else {
-    std::cout << "  unknown thing [" << (int)msg->networkId << "/"
-              << (int)msg->thingId << "]";
-  }
-  std::cout << std::endl;
 }
 
 // Receive

Participants/ParticipantUDP.h

@@ -1,13 +1,15 @@
 #pragma once
 
 #include "Messages/BinaryMsg.h"
+#include "Messages/DestroyMsg.h"
 #include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
-#include "Messages/SiteMsg.h"
+#include "Messages/NetworkIdMsg.h"
 #include "Messages/ThingMsg.h"
+#include "Messages/TextMsg.h"
 #include "Participant.h"
 
 #if !defined(NO_STD)
@@ -29,7 +31,8 @@ namespace RoboidControl {
 
 constexpr int MAX_SENDER_COUNT = 256;
 
-/// @brief A local participant is the local device which can communicate with
+/// @brief A participant using UDP communication
+/// A local participant is the local device which can communicate with
 /// other participants It manages all local things and communcation with other
 /// participants. Each application has a local participant which is usually
 /// explicit in the code. An participant can be isolated. In that case it is
@@ -41,6 +44,9 @@ constexpr int MAX_SENDER_COUNT = 256;
 /// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
 class ParticipantUDP : public Participant {
+
+#pragma region Init
+
  public:
   /// @brief Create a participant without connecting to a site
   /// @param port The port on which the participant communicates
@@ -51,41 +57,28 @@ class ParticipantUDP : public Participant {
   /// @brief Create a participant which will try to connect to a site.
   /// @param ipAddress The IP address of the site
   /// @param port The port used by the site
-  ParticipantUDP(const char* ipAddress,
+  /// @param localPort The port used by the local participant
-                   int port = 7681,
+  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
-                   int localPort = 7681);
-  // Note to self: one cannot specify the port used by the local participant
-  // now!!
 
-  /// @brief Isolated participant is used when the application is run without
+#pragma endregion Init
-  /// networking
-  /// @return A participant without networking support
-  static ParticipantUDP* Isolated();
 
+#pragma region Properties
+
+public:
   /// @brief True if the participant is running isolated.
   /// Isolated participants do not communicate with other participants
   bool isIsolated = false;
 
+  /// @brief The remote site when this participant is connected to a site
+  Participant* remoteSite = nullptr;
+
   /// The interval in milliseconds for publishing (broadcasting) data on the
   /// local network
   long publishInterval = 3000;  // 3 seconds
 
-  /// @brief The name of the participant
+ protected:
-  const char* name = "ParticipantUDP";
-
-  // int localPort = 0;
-
-  /// @brief The remote site when this participant is connected to a site
-  Participant* remoteSite = nullptr;
-
-#if defined(ARDUINO)
-  // const char* remoteIpAddress = nullptr;
-  // unsigned short remotePort = 0;
-  // char* broadcastIpAddress = nullptr;
-
-  // WiFiUDP udp;
-#else
 
+#if !defined(ARDUINO)
 #if defined(__unix__) || defined(__APPLE__)
   int sock;
 #elif defined(_WIN32) || defined(_WIN64)
@@ -93,81 +86,63 @@ class ParticipantUDP : public Participant {
   sockaddr_in server_addr;
   sockaddr_in broadcast_addr;
 #endif
-
 #endif
-
+ public:
   void begin();
   bool connected = false;
 
-  virtual void Update(unsigned long currentTimeMs = 0) override;
+#pragma endregion Properties
+
+#pragma region Update
+
+ public:
+  virtual void Update() override;
+
+ protected:
+  unsigned long nextPublishMe = 0;
+
+  /// @brief Prepare the local things for the next update
+  //virtual void PrepMyThings();
+  virtual void UpdateMyThings();
+  virtual void UpdateOtherThings();
+
+#pragma endregion Update
+
+#pragma region Send
 
   void SendThingInfo(Participant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
 
-  bool Send(Participant* remoteParticipant, IMessage* msg);
+  virtual bool Send(IMessage* msg) override;
   bool Publish(IMessage* msg);
 
+#pragma endregion Send
+
+#pragma region Receive
+
+protected:
   void ReceiveData(unsigned char bufferSize,
                    char* senderIpAddress,
                    unsigned int senderPort);
   void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
 
-#if defined(NO_STD)
-  unsigned char senderCount = 0;
-  Participant* senders[MAX_SENDER_COUNT];
-#else
-  std::list<Participant*> senders;
-#endif
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  char buffer[1024];
-
   void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
 
-  Participant* GetParticipant(const char* ipAddress, int port);
-  Participant* AddParticipant(const char* ipAddress, int port);
-
   void ReceiveUDP();
 
   virtual void Process(Participant* sender, ParticipantMsg* msg);
-  virtual void Process(Participant* sender, SiteMsg* msg);
+  virtual void Process(Participant* sender, NetworkIdMsg* msg);
   virtual void Process(Participant* sender, InvestigateMsg* msg);
   virtual void Process(Participant* sender, ThingMsg* msg);
   virtual void Process(Participant* sender, NameMsg* msg);
   virtual void Process(Participant* sender, ModelUrlMsg* msg);
   virtual void Process(Participant* sender, PoseMsg* msg);
   virtual void Process(Participant* sender, BinaryMsg* msg);
+  virtual void Process(Participant* sender, TextMsg* msg);
+  virtual void Process(Participant* sender, DestroyMsg* msg);
 
-#if !defined(NO_STD)
+#pragma endregion Receive
-//  public:
-//   using ThingConstructor = std::function<Thing*(Participant* participant,
-//                                                 unsigned char networkId,
-//                                                 unsigned char thingId)>;
 
-//   template <typename ThingClass>
-//   void Register(unsigned char thingType) {
-//     thingMsgProcessors[thingType] = [](Participant* participant,
-//                                        unsigned char networkId,
-//                                        unsigned char thingId) {
-//       return new ThingClass(participant, networkId, thingId);
-//     };
-//   };
-
-//   template <typename ThingClass>
-//   void Register2(unsigned char thingType, ThingConstructor f) {
-//     thingMsgProcessors[thingType] = [f](Participant* participant,
-//                                        unsigned char networkId,
-//                                        unsigned char thingId) {
-//       return f(participant, networkId, thingId);
-//     };
-//   };
-
-//  protected:
-//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
-
-#endif
 };
 
 }  // namespace RoboidControl

Participants/SiteServer.cpp

@@ -9,58 +9,89 @@
 
 namespace RoboidControl {
 
-SiteServer::SiteServer(int port) {
+#pragma region Init
+
+SiteServer::SiteServer(int port) : ParticipantUDP(port) {
   this->name = "Site Server";
   this->publishInterval = 0;
 
-  this->ipAddress = "0.0.0.0";
-  this->port = port;
-
-#if defined(NO_STD)
-  this->senders[this->senderCount++] = this;
-#else
-  this->senders.push_back(this);
-#endif
-
   SetupUDP(port, ipAddress, 0);
-
-#if !defined(NO_STD)
-  // Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
-#endif
 }
 
+#pragma endregion Init
+
+#pragma region Update
+
+void SiteServer::UpdateMyThings() {
+  for (Thing* thing : this->things) {
+    if (thing == nullptr)
+      continue;
+
+    thing->Update(true);
+
+    if (this->isIsolated == false) {
+      // Send to all other participants
+#if defined(NO_STD)
+      Participant** participants = Participant::registry.GetAll();
+      for (int ix = 0; ix < Participant::registry.count; ix++) {
+        Participant* participant = participants[ix];
+#else
+      for (Participant* participant : Participant::registry.GetAll()) {
+#endif
+        if (participant == nullptr || participant == this)
+          continue;
+
+        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+        participant->Send(poseMsg);
+        delete poseMsg;
+        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+        participant->Send(binaryMsg);
+        delete binaryMsg;
+      }
+    }
+  }
+}
+
+#pragma endregion Update
+
+#pragma region Receive
+
 void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
-  if (msg->networkId == 0) {
+  if (msg->networkId != sender->networkId) {
     // std::cout << this->name << " received New Client -> " <<
     // sender->ipAddress
     //           << ":" << (int)sender->port << "\n";
-    SiteMsg* msg = new SiteMsg(sender->networkId);
+    NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
-    this->Send(sender, msg);
+    sender->Send(msg);
     delete msg;
   }
 }
 
-void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
+void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
 
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->thingId);
-  if (thing == nullptr) {
+  if (thing == nullptr)
-    // #if defined(NO_STD)
+    // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
-    new Thing(sender, (Thing::Type)msg->thingType,
+    // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
-              msg->thingId);
+    //thing = new Thing(msg->thingType, sender->root);
-    // #else
+    ;
-    //     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
+  thing->id = msg->thingId;
-    //     //Thing* newThing;
+
-    //     if (thingMsgProcessor != thingMsgProcessors.end())  // found item
+  if (msg->parentId != 0) {
-    //       //newThing =
+    thing->SetParent(Get(msg->parentId));
-    //           thingMsgProcessor->second(sender, msg->networkId,
+    if (thing->IsRoot())
-    //           msg->thingId);
+    // if (thing->GetParent() != nullptr)
-    //     else
+#if defined(NO_STD)
-    //       //newThing =
+      ;
-    //         new Thing(sender, msg->networkId, msg->thingId,
+#else
-    //                            (Thing::Type)msg->thingType);
+      std::cout << "Could not find parent [" << (int)msg->networkId << "/"
-    // #endif
+                << (int)msg->parentId << "]\n";
-  }
+#endif
+  } else
+    thing->SetParent(Thing::LocalRoot());
 }
 
+#pragma endregion Receive
+
 }  // namespace RoboidControl

Participants/SiteServer.h

@@ -2,45 +2,43 @@
 
 #include "ParticipantUDP.h"
 
-#if !defined(NO_STD)
+// #if !defined(NO_STD)
-#include <functional>
+// #include <functional>
-#include <memory>
+// #include <memory>
-#include <unordered_map>
+// #include <unordered_map>
-#endif
+// #endif
 
 namespace RoboidControl {
 
 /// @brief A participant is device which can communicate with other participants
 class SiteServer : public ParticipantUDP {
+  
+#pragma region Init
+
  public:
+  /// @brief Create a new site server
+  /// @param port The port of which to receive the messages
   SiteServer(int port = 7681);
 
-  // virtual void Update(unsigned long currentTimeMs = 0) override;
+#pragma endregion Init
 
-// #if !defined(NO_STD)
+#pragma region Update
-//   template <typename ThingClass>
+
-//   void Register(unsigned char thingType) {
+  virtual void UpdateMyThings() override;
-//     thingMsgProcessors[thingType] = [](Participant* participant,
+
-//                                        unsigned char networkId,
+#pragma endregion Update
-//                                        unsigned char thingId) {
+
-//       return new ThingClass(participant, networkId, thingId);
+#pragma region Receive
-//     };
-//   };
-// #endif
 
  protected:
   unsigned long nextPublishMe = 0;
 
   virtual void Process(Participant* sender, ParticipantMsg* msg) override;
-  virtual void Process(Participant* sender, SiteMsg* msg) override;
+  virtual void Process(Participant* sender, NetworkIdMsg* msg) override;
   virtual void Process(Participant* sender, ThingMsg* msg) override;
 
-// #if !defined(NO_STD)
+#pragma endregion Receive
-//   using ThingConstructor = std::function<Thing*(Participant* participant,
+
-//                                                 unsigned char networkId,
-//                                                 unsigned char thingId)>;
-//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
-// #endif
 };
 
 }  // namespace RoboidControl

Posix/PosixParticipant.cpp

@@ -11,7 +11,7 @@
 namespace RoboidControl {
 namespace Posix {
 
-void Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(__unix__) || defined(__APPLE__)
 
   // Create a UDP socket
@@ -63,7 +63,7 @@ void Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #endif
 }
 
-void Receive() {
+void ParticipantUDP::Receive() {
 #if defined(__unix__) || defined(__APPLE__)
   sockaddr_in client_addr;
   socklen_t len = sizeof(client_addr);
@@ -74,9 +74,9 @@ void Receive() {
     unsigned int sender_port = ntohs(client_addr.sin_port);
 
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    // RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port);
     // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
     //   //           << "\n";
     //   // std::cout << "New remote participant " << remoteParticipant->ipAddress
@@ -90,7 +90,7 @@ void Receive() {
 #endif
 }
 
-bool Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
   // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
   //           << "\n";
@@ -113,7 +113,7 @@ bool Send(Participant* remoteParticipant, int bufferSize) {
   return true;
 }
 
-bool Publish(IMessage* msg) {
+bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(__unix__) || defined(__APPLE__)
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)

Posix/PosixParticipant.h

@@ -11,6 +11,13 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
+
+ protected:
+#if defined(__unix__) || defined(__APPLE__)
+  sockaddr_in remote_addr;
+  sockaddr_in server_addr;
+  sockaddr_in broadcast_addr;
+#endif
 };
 
 }  // namespace Posix

README.md

@@ -14,5 +14,60 @@ Supporting:
 # Basic components
 
 - RoboidControl::Thing
-- RoboidControl::LocalParticipant
+- RoboidControl::Participant
-- RoboidControl::SiteServer
+
+# Installation
+
+## Core code
+
+The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`.
+For example if the library is placed in the subfolder `roboidcontrol`:
+```
+# Add the path to Roboid Control
+add_subdirectory(roboidcontrol)
+
+# Your source files/executable
+add_executable(my_executable main.cpp) 
+
+# Link against RoboidControl
+target_link_libraries(my_executable RoboidControl)
+```
+
+## Arduino (PlatformIO)
+
+Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported.
+
+The best way to include support for Roboid Control in PlatformIO is
+to clone the Roboid Control for C++ repository into a subfolder of the /lib folder.
+Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder.
+
+## ESP-IDF
+
+The best way to include support for Roboid Control in PlatformIO is
+to clone the Roboid Control for C++ repository into a subfolder of the /components folder.
+Alternatively you can download the zip file and unpack it as a subfolder of the /components folder.
+
+Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example:
+```
+list(APPEND EXTRA_COMPONENT_DIRS
+    components/RoboidControl    
+    )
+```
+
+# Get Started
+
+## Core C++ Examples
+
+This repository contains examples in the `examples` folder. You can build these using cmake.
+
+For example, to build the BB2A example:
+``` 
+cmake -B build -D BUILD_EXAMPLE_BB2A=ON
+cmake --build build
+```
+The resulting executable is then `build/examples/Debug/BB2A.exe`
+
+## Arduino (PlatformIO) Examples
+
+Specific examples for the Arduino platform are found in the `Arduino\examples` folder.
+To use them you should create a new project in PlatformIO and then copy the example code to your project.

Testing/Temporary/LastTest.log

@@ -0,0 +1,3 @@
+Start testing: Jun 17 17:17 W. Europe Summer Time
+----------------------------------------------------------
+End testing: Jun 17 17:17 W. Europe Summer Time

Thing.cpp

@@ -5,6 +5,7 @@
 #include "Participants/IsolatedParticipant.h"
 
 #include <string.h>
+// #include <iostream>
 
 #if defined(ARDUINO)
 #include "Arduino.h"
@@ -17,72 +18,78 @@
 
 namespace RoboidControl {
 
-// LocalParticipant* Thing::CheckHiddenParticipant() {
+#pragma region Init
-//   if (isolatedParticipant == nullptr)
-//     isolatedParticipant = new LocalParticipant(0);
-//   return isolatedParticipant;
-// }
 
-Thing::Thing(int thingType)
+Thing* Thing::LocalRoot() {
-    : Thing(IsolatedParticipant::Isolated(), thingType) {}
+  Participant* p = Participant::LocalParticipant;
+  Thing* localRoot = p->root;
+  return localRoot;
+}
 
-Thing::Thing(Participant* owner, Type thingType, unsigned char thingId)
+// Only use this for root things
-    : Thing(owner, (unsigned char)thingType, thingId) {}
+Thing::Thing(Participant* owner) {
-
+  this->type = Type::Root;
-Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
+  this->name = "Root";
-  this->owner = owner;
-  this->id = thingId;
-  this->type = thingType;
-  this->networkId = 0;
 
   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;
 
-  // std::cout << "add thing [" << (int)this->id << "] to owner "
+  this->owner = owner;
-  //           << this->owner->ipAddress << ":" << this->owner->port << std::endl;
+  this->owner->Add(this);
+  // std::cout << this->owner->name << ": New root thing " << std::endl;
+}
+
+void Thing::CreateRoot(Participant* owner) {
+  owner->root = new Thing(owner);
+}
+
+Thing::Thing(Thing* parent) {
+  this->type = Type::Undetermined;
+
+  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 = parent->owner;
   this->owner->Add(this, true);
+  this->SetParent(parent);
+
+  std::cout << this->owner->name << ": New thing for " << parent->name
+            << std::endl;
 }
 
-// Thing::Thing(Participant* owner,
+Thing::~Thing() {
-//             Type thingType,
+  std::cout << "Destroy thing " << this->name << std::endl;
-//              int thingId) {
-//   // no participant reference yet..
-//   this->owner = owner;
-//   this->networkId = networkId;
-//   this->id = thingId;
-//   this->type = (unsigned char)thingType;
-
-//   this->linearVelocity = Spherical::zero;
-//   this->angularVelocity = Spherical::zero;
-//   // std::cout << "Created thing " << (int)this->networkId << "/" <<
-//   // (int)this->id
-//   //           << "\n";
-//   owner->Add(this, false);
-// }
-
-void Thing::Terminate() {
-  // Thing::Remove(this);
 }
 
-Thing* Thing::FindThing(const char* name) {
+#pragma endregion Init
-  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-    Thing* child = this->children[childIx];
-    if (child == nullptr || child->name == nullptr)
-      continue;
 
-    if (strcmp(child->name, name) == 0)
+void Thing::SetName(const char* name) {
-      return child;
+  this->name = name;
-
+  this->nameChanged = true;
-    Thing* foundChild = child->FindThing(name);
-    if (foundChild != nullptr)
-      return foundChild;
-  }
-  return nullptr;
 }
 
+const char* Thing::GetName() const {
+  return this->name;
+}
+
+void Thing::SetModel(const char* url) {
+  this->modelUrl = url;
+}
+
+#pragma region Hierarchy
+
 void Thing::SetParent(Thing* parent) {
   if (parent == nullptr) {
     Thing* parentThing = this->parent;
@@ -91,18 +98,21 @@ void Thing::SetParent(Thing* parent) {
     this->parent = nullptr;
   } else
     parent->AddChild(this);
+  this->hierarchyChanged = true;
 }
 
-void Thing::SetParent(Thing* root, const char* name) {
+bool Thing::IsRoot() const {
-  Thing* thing = root->FindThing(name);
+  return this == LocalRoot() || this->parent == nullptr;
-  if (thing != nullptr)
-    this->SetParent(thing);
 }
 
 Thing* Thing::GetParent() {
   return this->parent;
 }
 
+Thing* Thing::GetChildByIndex(unsigned char ix) {
+  return this->children[ix];
+}
+
 void Thing::AddChild(Thing* child) {
   unsigned char newChildCount = this->childCount + 1;
   Thing** newChildren = new Thing*[newChildCount];
@@ -142,7 +152,7 @@ Thing* Thing::RemoveChild(Thing* child) {
     }
   }
 
-  child->parent = nullptr;
+  child->parent = Thing::LocalRoot();
 
   delete[] this->children;
   this->children = newChildren;
@@ -151,7 +161,7 @@ Thing* Thing::RemoveChild(Thing* child) {
   return child;
 }
 
-Thing* Thing::GetChild(unsigned char id, bool recursive) {
+Thing* Thing::GetChild(unsigned char id, bool recurse) {
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
     Thing* child = this->children[childIx];
     if (child == nullptr)
@@ -159,8 +169,8 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) {
     if (child->id == id)
       return child;
 
-    if (recursive) {
+    if (recurse) {
-      Thing* foundChild = child->GetChild(id, recursive);
+      Thing* foundChild = child->GetChild(id, recurse);
       if (foundChild != nullptr)
         return foundChild;
     }
@@ -168,57 +178,25 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) {
   return nullptr;
 }
 
-Thing* Thing::GetChildByIndex(unsigned char ix) {
+Thing* Thing::FindChild(const char* name, bool recurse) {
-  return this->children[ix];
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-}
+    Thing* child = this->children[childIx];
+    if (child == nullptr || child->name == nullptr)
+      continue;
 
-void Thing::SetModel(const char* url) {
+    if (strcmp(child->name, name) == 0)
-  this->modelUrl = url;
+      return child;
-}
 
-unsigned long Thing::GetTimeMs() {
+    Thing* foundChild = child->FindChild(name);
-#if defined(ARDUINO)
+    if (foundChild != nullptr)
-  return millis();
+      return foundChild;
-#else
-  auto now = std::chrono::steady_clock::now();
-  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-      now.time_since_epoch());
-  return static_cast<unsigned long>(ms.count());
-#endif
-}
-
-void Thing::Update(bool recursive) {
-  Update(GetTimeMs(), recursive);
-}
-
-void Thing::Update(unsigned long currentTimeMs, bool recursive) {
-  // if (this->positionUpdated || this->orientationUpdated)
-  //   OnPoseChanged callback
-  this->positionUpdated = false;
-  this->orientationUpdated = false;
-
-  if (recursive) {
-    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-      Thing* child = this->children[childIx];
-      if (child == nullptr)
-        continue;
-      child->Update(currentTimeMs, recursive);
-    }
   }
+  return nullptr;
 }
 
-void Thing::UpdateThings(unsigned long currentTimeMs) {
+#pragma endregion Hierarchy
-  IsolatedParticipant::Isolated()->Update(currentTimeMs);
-}
 
-int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
+#pragma region Pose
-  (void)buffer;
-  (void)ix;
-  return 0;
-}
-void Thing::ProcessBinary(char* bytes) {
-  (void)bytes;
-};
 
 void Thing::SetPosition(Spherical position) {
   this->position = position;
@@ -238,8 +216,10 @@ SwingTwist Thing::GetOrientation() {
 }
 
 void Thing::SetLinearVelocity(Spherical linearVelocity) {
-  this->linearVelocity = linearVelocity;
+  if (this->linearVelocity.distance != linearVelocity.distance) {
-  this->linearVelocityUpdated = true;
+    this->linearVelocity = linearVelocity;
+    this->linearVelocityUpdated = true;
+  }
 }
 
 Spherical Thing::GetLinearVelocity() {
@@ -247,12 +227,60 @@ Spherical Thing::GetLinearVelocity() {
 }
 
 void Thing::SetAngularVelocity(Spherical angularVelocity) {
-  this->angularVelocity = angularVelocity;
+  if (this->angularVelocity.distance != angularVelocity.distance) {
-  this->angularVelocityUpdated = true;
+    this->angularVelocity = angularVelocity;
+    this->angularVelocityUpdated = true;
+  }
 }
 
 Spherical Thing::GetAngularVelocity() {
   return this->angularVelocity;
 }
 
+#pragma endregion Pose
+
+#pragma region Update
+
+unsigned long Thing::GetTimeMs() {
+#if defined(ARDUINO)
+  unsigned long ms = millis();
+  return ms;
+#else
+  auto now = std::chrono::steady_clock::now();
+  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+      now.time_since_epoch());
+  return static_cast<unsigned long>(ms.count());
+#endif
+}
+
+void Thing::Update(bool recursive) {
+  this->positionUpdated = false;
+  this->orientationUpdated = false;
+  this->linearVelocityUpdated = false;
+  this->angularVelocityUpdated = false;
+  this->hierarchyChanged = false;
+  this->nameChanged = false;
+
+  if (recursive) {
+    // std::cout << "# children: " << (int)this->childCount << std::endl;
+    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+      Thing* child = this->children[childIx];
+      if (child == nullptr)
+        continue;
+      child->Update(recursive);
+    }
+  }
+}
+
+#pragma endregion Update
+
+int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
+  (void)buffer;
+  (void)ix;
+  return 0;
+}
+void Thing::ProcessBinary(char* bytes) {
+  (void)bytes;
+};
+
 }  // namespace RoboidControl

Thing.h

@@ -20,71 +20,117 @@ class ParticipantUDP;
 class Thing {
  public:
   /// @brief Predefined thing types
-  enum Type {
+  struct Type {
-    Undetermined,
+    static const unsigned char Undetermined = 0x00;
-    // Sensor,
+    // Sensor
-    Switch,
+    static const unsigned char Switch = 0x01;
-    DistanceSensor,
+    static const unsigned char DistanceSensor = 0x02;
-    DirectionalSensor,
+    static const unsigned char DirectionalSensor = 0x03;
-    TemperatureSensor,
+    static const unsigned char TemperatureSensor = 0x04;
-    TouchSensor,
+    static const unsigned char TouchSensor = 0x05;
-    // Motor,
+    // Motor
-    ControlledMotor,
+    static const unsigned char ControlledMotor = 0x06;
-    UncontrolledMotor,
+    static const unsigned char UncontrolledMotor = 0x07;
-    Servo,
+    static const unsigned char Servo = 0x08;
+    static const unsigned char RelativeEncoder = 0x19;
     // Other
-    Roboid,
+    static const unsigned char Root = 0x10;
-    Humanoid,
+    static const unsigned char Roboid = 0x09;
-    ExternalSensor,
+    static const unsigned char Humanoid = 0x0A;
+    static const unsigned char ExternalSensor = 0x08;
+    static const unsigned char Animator = 0x0C;
+    static const unsigned char DifferentialDrive = 0x0D;
   };
 
-  /// @brief Create a new thing using an implicit local participant
+#pragma region Init
-  /// @param thingType The type of thing
-  Thing(int thingType = Type::Undetermined);
-  /// @brief Create a new thing of the given type
-  /// @param thingType The predefined type of thing
-  Thing(Participant* participant,
-        Type thingType = Type::Undetermined,
-        unsigned char thingId = 0);
-  /// @brief Create a new thing of the give type
-  /// @param thingType The custom type of the thing
-  Thing(Participant* participant, int thingType, unsigned char thingId = 0);
-  /// @brief Create a new thing for the given participant
-  /// @param participant The participant for which this thing is created
-  /// @param networkId The network ID of the thing
-  /// @param thingId The ID of the thing
-  /// @param thingType The type of thing
-  // Thing(Participant* participant,
-  //       unsigned char networkId,
-  //       unsigned char thingId,
-  //       Type thingType = Type::Undetermined);
 
-  /// @brief The participant managing this thing
+ private:
-  Participant* owner;
+ public:
-  /// @brief The network ID of this thing
+  /// @brief Create a new Thing
-  /// @note This field will likely disappear in future versions
+  /// @param parent (optional) The parent thing
-  unsigned char networkId = 0;
+  /// The owner will be the same as the owner of the parent thing, it will
+  /// be Participant::LocalParticipant if the parent is not specified. A thing
+  /// without a parent will be connected to the root thing.
+  Thing(Thing* parent = LocalRoot());
+
+ private:
+  /// @brief Constructor to create a root thing
+  /// @param owner The participant who will own this root thing
+  /// @remarks This function is private because CreateRoot() should be used
+  /// instead
+  Thing(Participant* owener);
+
+ public:
+  /// @brief Destructor for a Thing
+  ~Thing();
+
+  /// @brief Create a root thing for a participant
+  /// @param owner The participant who will own this root thing
+  static void CreateRoot(Participant* owner);
+
+  /// @brief The root thing for the local participant
+  /// @return The root thing for the local participant
+  static Thing* LocalRoot();
+
+#pragma endregion Init
+
+ public:
+  /// @brief Terminated things are no longer updated
+  bool terminate = false;
+
+#pragma region Properties
+
+ public:
   /// @brief The ID of the thing
   unsigned char id = 0;
 
   /// @brief The type of Thing
   /// This can be either a Thing::Type of a byte value for custom types
-  unsigned char type = 0;
+  unsigned char type = Type::Undetermined;
 
-  /// @brief Find a thing by name
+  /// @brief The participant owning this thing
-  /// @param name Rhe name of the thing
+  Participant* owner = nullptr;
-  /// @return The found thing or nullptr when nothing is found
-  Thing* FindThing(const char* name);
 
-  /// @brief Sets the parent Thing
+  /// @brief The name of the thing
-  /// @param parent The Thing which should become the parnet
+  const char* name = nullptr;
-  /// @remark This is equivalent to calling parent->AddChild(this);
+
-  virtual void SetParent(Thing* parent);
+  void SetName(const char* name);
-  void SetParent(Thing* root, const char* name);
+  const char* GetName() const;
-  /// @brief Gets the parent Thing
+  bool nameChanged = false;
+
+  /// @brief Sets the location from where the 3D model of this Thing can be
+  /// loaded from
+  /// @param url The url of the model
+  /// @remark Although the roboid implementation is not dependent on the model,
+  /// the only official supported model formats are .png (sprite), .gltf and .glb
+  void SetModel(const char* url);
+
+  /// @brief An URL pointing to the location where a model of the thing can be
+  /// found
+  const char* modelUrl = nullptr;
+
+#pragma endregion Properties
+
+#pragma region Hierarchy
+
+  /// @brief Sets the parent of this Thing
+  /// @param parent The Thing which should become the parent
+  void SetParent(Thing* parent);
+  /// @brief Gets the parent of this Thing
   /// @return The parent Thing
   Thing* GetParent();
 
+  /// @brief Check if this is a root thing
+  /// @return True is this thing is a root
+  bool IsRoot() const;
+
+  /// @brief The number of children
+  unsigned char childCount = 0;
+  /// @brief Get a child by index
+  /// @param ix The child index
+  /// @return The found thing of nullptr when nothing is found
+  Thing* GetChildByIndex(unsigned char ix);
+
   /// @brief Add a child Thing to this Thing
   /// @param child The Thing which should become a child
   /// @remark When the Thing is already a child, it will not be added again
@@ -94,49 +140,46 @@ class Thing {
   /// @return The removed child or nullptr if the child could not be found
   Thing* RemoveChild(Thing* child);
 
-  /// @brief The number of children
-  unsigned char childCount = 0;
   /// @brief Get a child by thing Id
   /// @param id The thing ID to find
-  /// @param recursive Look recursively through all descendants
+  /// @param recurse Look recursively through all descendants
   /// @return The found thing of nullptr when nothing is found
-  Thing* GetChild(unsigned char id, bool recursive = false);
+  Thing* GetChild(unsigned char id, bool recurse = false);
-  /// @brief Get a child by index
-  /// @param ix The child index
-  /// @return The found thing of nullptr when nothing is found
-  Thing* GetChildByIndex(unsigned char ix);
 
- protected:
+  /// @brief Find a thing by name
+  /// @param name The name of the thing
+  /// @param recurse Look recursively through all descendants
+  /// @return The found thing or nullptr when nothing is found
+  Thing* FindChild(const char* name, bool recurse = true);
+
+  /// @brief Indicator that the hierarchy of the thing has changed
+  bool hierarchyChanged = true;
+
+ private:
   Thing* parent = nullptr;
   Thing** children = nullptr;
 
- public:
+#pragma endregion Hierarchy
-  /// @brief The name of the thing
-  const char* name = nullptr;
-  /// @brief An URL pointing to the location where a model of the thing can be
-  /// found
-  const char* modelUrl = nullptr;
-  /// @brief The scale of the model (deprecated I think)
-  float modelScale = 1;
 
+#pragma region Pose
+
+ public:
   /// @brief Set the position of the thing
   /// @param position The new position in local space, in meters
   void SetPosition(Spherical position);
   /// @brief Get the position of the thing
   /// @return The position in local space, in meters
   Spherical GetPosition();
+  /// @brief Boolean indicating that the thing has an updated position
+  bool positionUpdated = false;
+
   /// @brief Set the orientation of the thing
   /// @param orientation The new orientation in local space
   void SetOrientation(SwingTwist orientation);
   /// @brief Get the orientation of the thing
   /// @return The orienation in local space
   SwingTwist GetOrientation();
-  /// @brief The scale of the thing (deprecated I think)
+  /// @brief Boolean indicating the thing has an updated orientation
-  // float scale = 1;  // assuming uniform scale
-
-  /// @brief boolean indicating if the position was updated
-  bool positionUpdated = false;
-  /// @brief boolean indicating if the orientation was updated
   bool orientationUpdated = false;
 
   /// @brief Set the linear velocity of the thing
@@ -146,57 +189,59 @@ class Thing {
   /// @brief Get the linear velocity of the thing
   /// @return The linear velocity in local space, in meters per second
   virtual Spherical GetLinearVelocity();
+  /// @brief Boolean indicating the thing has an updated linear velocity
+  bool linearVelocityUpdated = false;
+
   /// @brief Set the angular velocity of the thing
   /// @param angularVelocity the new angular velocity in local space
   virtual void SetAngularVelocity(Spherical angularVelocity);
   /// @brief Get the angular velocity of the thing
   /// @return The angular velocity in local space
   virtual Spherical GetAngularVelocity();
-  bool linearVelocityUpdated = false;
+  /// @brief Boolean indicating the thing has an updated angular velocity
   bool angularVelocityUpdated = false;
 
  private:
-  /// @brief The position in local space
+  /// @brief The position of the thing in local space, in meters
   /// @remark When this Thing has a parent, the position is relative to the
   /// parent's position and orientation
   Spherical position;
-  /// @brief The orientation in local space
+  /// @brief The orientation of the thing in local space
   /// @remark When this Thing has a parent, the orientation is relative to the
   /// parent's orientation
   SwingTwist orientation;
 
-  /// @brief The linear velocity in local space
+  /// @brief The linear velocity of the thing in local space, in meters per
+  /// second
   Spherical linearVelocity;
-  /// @brief The angular velocity in local spze
+  /// @brief The angular velocity of the thing in local space, in degrees per
+  /// second
   Spherical angularVelocity;
 
+#pragma endregion Pose
+
+#pragma region Update
+
  public:
-  /// @brief Terminated things are no longer updated
+  //virtual void PrepareForUpdate();
-  void Terminate();
-
-  /// @brief Sets the location from where the 3D model of this Thing can be
-  /// loaded from
-  /// @param url The url of the model
-  /// @remark Although the roboid implementation is not dependent on the model,
-  /// the only official supported model format is .obj
-  void SetModel(const char* url);
-
-  static unsigned long GetTimeMs();
-
-  void Update(bool recursive = false);
 
   /// @brief Updates the state of the thing
-  /// @param currentTimeMs The current clock time in milliseconds
+  /// @param recurse When true, this will Update the descendants recursively
-  virtual void Update(unsigned long currentTimeMs, bool recursive = false);
+  virtual void Update(bool recurse = false);
 
-  static void UpdateThings(unsigned long currentTimeMs);
+  /// @brief Get the current time in milliseconds
+  /// @return The current time in milliseconds
+  static unsigned long GetTimeMs();
 
+#pragma endregion Update
+
+ public:
   /// @brief Function used to generate binary data for this thing
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  /// @returns The size of the binary data
+  /// @return The size of the binary data
   virtual int GenerateBinary(char* buffer, unsigned char* ix);
-  // /// @brief FUnction used to process binary data received for this thing
+  /// @brief Function used to process binary data received for this thing
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes);
 };

Things/ControlledMotor.cpp

@@ -0,0 +1,68 @@
+#include "ControlledMotor.h"
+#include "LinearAlgebra/FloatSingle.h"
+
+namespace RoboidControl {
+
+ControlledMotor::ControlledMotor(Motor* motor,
+                                 RelativeEncoder* encoder,
+                                 Thing* parent)
+    : Motor(parent), motor(motor), encoder(encoder) {
+  this->type = Type::ControlledMotor;
+  //encoder->SetParent(null);
+  //  Thing parent = motor.GetParent();
+  //  this->SetParent(parent);
+  this->integral = 0;
+}
+
+void ControlledMotor::SetTargetVelocity(float velocity) {
+  this->targetVelocity = velocity;
+  this->rotationDirection =
+      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
+}
+
+void ControlledMotor::Update(bool recurse) {
+  unsigned long currentTimeMs = GetTimeMs();
+  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
+  this->lastUpdateTime = currentTimeMs;
+  if (timeStep <= 0)
+    return;
+
+  // encoder->Update(false);
+
+  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
+
+  float error = this->targetVelocity - this->actualVelocity;
+  float p_term = error * pidP;
+  this->integral += error * timeStep;
+  float i_term = pidI * this->integral;
+  float d_term = pidD * (error - this->lastError) / timeStep;
+  this->lastError = error;
+
+  float output = p_term + i_term + d_term;
+  // std::cout << "target " << this->targetVelocity << " actual "
+  //           << this->actualVelocity << " output = " << output << std::endl;
+  // float acceleration =
+  //     error * timeStep * pidP;  // Just P is used at this moment
+  // std::cout << "motor acc. " << acceleration << std::endl;
+
+  // float newTargetVelocity = motor->targetVelocity + acceleration;
+  output = LinearAlgebra::Float::Clamp(output, -1, 1);
+  motor->SetTargetVelocity(output);  // or something like that
+  //motor->Update(false);
+}
+
+// float ControlledMotor::GetActualVelocity() {
+//   return (int)rotationDirection * encoder->rotationSpeed;
+// }
+
+// bool ControlledMotor::Drive(float distance) {
+//   if (!driving) {
+//     targetDistance = distance;
+//     startDistance = encoder->GetDistance();
+//     driving = true;
+//   }
+//   float totalDistance = encoder->GetDistance() - startDistance;
+//   bool completed = totalDistance > targetDistance;
+//   return completed;
+// }
+}  // namespace RoboidControl

Things/ControlledMotor.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include "Motor.h"
+#include "RelativeEncoder.h"
+
+namespace RoboidControl {
+
+/// @brief A motor with speed control
+/// It uses a feedback loop from an encoder to regulate the speed
+/// The speed is measured in revolutions per second.
+class ControlledMotor : public Motor {
+ public:
+  ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
+
+  float pidP = 0.5;
+  float pidD = 0;
+  float pidI = 0.2;
+
+  /// @brief The actual velocity in revolutions per second
+  float actualVelocity;
+
+  enum Direction { Forward = 1, Reverse = -1 };
+  Direction rotationDirection;
+
+  virtual void Update(bool recurse = false) override;
+
+  /// @brief Set the target verlocity for the motor controller
+  /// @param speed the target velocity in revolutions per second
+  virtual void SetTargetVelocity(float velocity) override;
+
+  Motor* motor;
+  RelativeEncoder* encoder;
+
+ protected:
+  float integral = 0;
+  float lastError = 0;
+  float lastUpdateTime;
+};
+
+}  // namespace RoboidControl

Things/DifferentialDrive.cpp

@@ -1,10 +1,29 @@
 #include "DifferentialDrive.h"
 
-namespace RoboidControl {
+#include "Messages/LowLevelMessages.h"
-DifferentialDrive::DifferentialDrive() : Thing() {}
 
-RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
+namespace RoboidControl {
-    : Thing(participant) {}
+
+DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) {
+  this->type = Type::DifferentialDrive;
+  this->name = "Differential drive";
+
+  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(parent) {
+  this->type = Type::DifferentialDrive;
+  this->name = "Differential drive";
+  this->leftWheel = leftMotor;
+  this->rightWheel = rightMotor;
+}
 
 void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
                                            float wheelSeparation) {
@@ -21,51 +40,66 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
+// Motor& DifferentialDrive::GetMotorLeft() {
+//   return *this->leftWheel;
+// }
+
+// Motor& DifferentialDrive::GetMotorRight() {
+//   return *this->rightWheel;
+// }
+
+void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
   float distance = this->wheelSeparation / 2;
-  this->leftWheel = leftWheel;
+  this->leftWheel = &leftMotor;
-  ;
+  this->leftWheel->SetPosition(Spherical(distance, Direction::left));
-  if (leftWheel != nullptr)
-    this->leftWheel->SetPosition(Spherical(distance, Direction::left));
 
-  this->rightWheel = rightWheel;
+  this->rightWheel = &rightMotor;
-  if (rightWheel != nullptr)
+  this->rightWheel->SetPosition(Spherical(distance, Direction::right));
-    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
+void DifferentialDrive::SetWheelVelocity(float velocityLeft,
+                                         float velocityRight) {
+  // if (this->leftWheel != nullptr)
+  //   this->leftWheel->SetAngularVelocity(Spherical(velocityLeft,
+  //   Direction::left));
+  // if (this->rightWheel != nullptr)
+  //   this->rightWheel->SetAngularVelocity(
+  //       Spherical(velocityRight, Direction::right));
   if (this->leftWheel != nullptr)
-    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
+    this->leftWheel->SetTargetVelocity(velocityLeft);
   if (this->rightWheel != nullptr)
-    this->rightWheel->SetAngularVelocity(
+    this->rightWheel->SetTargetVelocity(velocityRight);
-        Spherical(speedRight, Direction::right));
 }
 
-void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
+void DifferentialDrive::Update(bool recursive) {
-  if (this->linearVelocityUpdated == false)
+  if (this->linearVelocityUpdated) {
-    return;
+    // this assumes forward velocity only....
-  // this assumes forward velocity only....
+    float linearVelocity = this->GetLinearVelocity().distance;
-  float linearVelocity = this->GetLinearVelocity().distance;
 
-  Spherical angularVelocity = this->GetAngularVelocity();
+    Spherical angularVelocity = this->GetAngularVelocity();
-  float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
+    float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
-  // Determine the rotation direction
+    // Determine the rotation direction
-  if (angularVelocity.direction.horizontal.InDegrees() < 0)
+    if (angularVelocity.direction.horizontal.InDegrees() < 0)
-    angularSpeed = -angularSpeed;
+      angularSpeed = -angularSpeed;
 
-  // wheel separation can be replaced by this->leftwheel->position->distance
+    // wheel separation can be replaced by this->leftwheel->position->distance
-  float speedLeft =
+    float speedLeft =
-      (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
+        (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
+        this->wheelRadius * Rad2Deg;
-  float speedRight =
+    float speedRight =
-      (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
+        (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
+        this->wheelRadius * Rad2Deg;
 
-  this->SetWheelVelocity(speedLeft, speedRight);
+    this->SetWheelVelocity(speedLeft, speedRight);
-  Thing::Update(currentMs, recursive);
+  }
-  // std::cout << "lin. speed " << linearVelocity << "   ang. speed " <<
+  Thing::Update(recursive);
-  // angularVelocity.distance << "   left wheel  "
+}
-  //         << speedLeft << "   right wheel " << speedRight << "\n";
+
+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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Thing.h"
+#include "Motor.h"
 
 namespace RoboidControl {
 
@@ -9,11 +10,13 @@ namespace RoboidControl {
 /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
 class DifferentialDrive : public Thing {
  public:
-  /// @brief Create a differential drive without networking support
+  /// @brief Create a new child differential drive
-  DifferentialDrive();
+  /// @param parent The parent thing
-  /// @brief Create a differential drive with networking support
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// @param participant The local participant
+  /// an ID
-  DifferentialDrive(Participant* participant);
+  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
@@ -23,35 +26,41 @@ class DifferentialDrive : public Thing {
   /// linear and angular velocity.
   /// @sa SetLinearVelocity SetAngularVelocity
   void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
+
+  // Motor& GetMotorLeft();
+  // 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
-  void SetMotors(Thing* leftWheel, Thing* rightWheel);
+  void SetMotors(Motor& leftMotor, Motor& rightMotor);
 
   /// @brief Directly specify the speeds of the motors
   /// @param speedLeft The speed of the left wheel in degrees per second.
   /// Positive moves the robot in the forward direction.
   /// @param speedRight The speed of the right wheel in degrees per second.
   /// Positive moves the robot in the forward direction.
-
   void SetWheelVelocity(float speedLeft, float speedRight);
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long)
-  virtual void Update(unsigned long currentMs, bool recursive = true) override;
+  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
-  Thing* leftWheel = nullptr;
-  /// @brief The right wheel
-  Thing* rightWheel = nullptr;
 };
 
 }  // namespace RoboidControl

Things/DigitalSensor.cpp

@@ -2,8 +2,17 @@
 
 namespace RoboidControl {
 
-//DigitalSensor::DigitalSensor() {}
+DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent) {
+  this->type = Type::Switch;
+}
 
-DigitalSensor::DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {}
+int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+  bytes[(*ix)++] = state ? 1 : 0;
+  return 1;
+}
+
+void DigitalSensor::ProcessBinary(char* bytes) {
+  this->state |= (bytes[0] == 1);
+}
 
 }  // namespace RoboidControl

Things/DigitalSensor.h

@@ -7,15 +7,31 @@ namespace RoboidControl {
 /// @brief A digital (on/off, 1/0, true/false) sensor
 class DigitalSensor : public Thing {
  public:
+  /// @brief Create a digital sensor without communication abilities
+  //DigitalSensor();
+  /// @brief Create a digital sensor for a participant
+  /// @param owner The owning participant
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// an ID
+  DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0);
+  /// @brief Create a new child digital sensor
+  /// @param parent The parent 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());
+
   /// @brief The sigital state
   bool state = 0;
 
-  /// @brief The default constructor
+  /// @brief Function used to generate binary data for this digital sensor
-  //DigitalSensor();
+  /// @param buffer The byte array for thw binary data
-  /// @brief Create a temperature sensor with the given ID
+  /// @param ix The starting position for writing the binary data
-  /// @param networkId The network ID of the sensor
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @param thingId The ID of the thing
+  /// @brief Function used to process binary data received for this digital
-  DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
+  /// sensor
+  /// @param bytes The binary data to process
+  virtual void ProcessBinary(char* bytes) override;
 };
 
 }  // namespace RoboidControl

Things/DistanceSensor.cpp

@@ -0,0 +1,29 @@
+#include "DistanceSensor.h"
+
+#include "Messages/LowLevelMessages.h"
+
+namespace RoboidControl {
+
+DistanceSensor::DistanceSensor(Thing* parent) : Thing(parent) {
+  this->type = Type::DistanceSensor;
+  this->name = "Distance sensor";
+}
+
+float DistanceSensor::GetDistance() {
+  if (this->externalDistance < this->internalDistance)
+    return this->externalDistance;
+  else
+    return this->internalDistance;
+}
+
+int DistanceSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+  LowLevelMessages::SendFloat16(bytes, ix, this->internalDistance);
+  return *ix;
+}
+
+void DistanceSensor::ProcessBinary(char* bytes) {
+  unsigned char ix = 0;
+  this->externalDistance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
+}
+
+}  // namespace RoboidControl

Things/DistanceSensor.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#if !NO_STD
+#include <limits>
+#endif
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+/// @brief A sensor measuring distance
+class DistanceSensor : public Thing {
+ public:
+  /// @brief Create a new child touch sensor
+  /// @param parent The parent thing
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// an ID
+  DistanceSensor(Thing* parent = Thing::LocalRoot());
+
+  /// @brief Get the current distance
+  float GetDistance();
+
+  /// @brief Function used to generate binary data for this sensor
+  /// @param buffer The byte array for thw binary data
+  /// @param ix The starting position for writing the binary data
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  /// @brief Function used to process binary data received for this sensor
+  /// @param bytes The binary data to process
+  virtual void ProcessBinary(char* bytes) override;
+
+ protected:
+#if ARDUNIO
+  float internalDistance = INFINITY;
+  float externalDistance = INFINITY;
+#else
+  float internalDistance = std::numeric_limits<double>::infinity();
+  float externalDistance = std::numeric_limits<double>::infinity();
+#endif
+};
+
+}  // namespace RoboidControl

Things/Motor.cpp

@@ -0,0 +1,34 @@
+#include "Motor.h"
+
+#include "Messages/BinaryMsg.h"
+#include "Participant.h"
+
+namespace RoboidControl {
+
+Motor::Motor(Thing* parent) : Thing(parent) {
+  this->type = Type::UncontrolledMotor;
+}
+
+void Motor::SetTargetVelocity(float targetSpeed) {
+  if (targetSpeed != this->targetVelocity) {
+    this->targetVelocity = targetSpeed;
+
+    if (this->owner->networkId != 0) {
+      // in other word: if we are connected...
+      BinaryMsg* binaryMsg = new BinaryMsg(this->owner->networkId, this);
+      this->owner->Send(binaryMsg);
+      delete binaryMsg;
+    }
+  }
+}
+
+float Motor::GetTargetVelocity() {
+  return this->targetVelocity;
+}
+
+int Motor::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = this->targetVelocity * 127.0f;
+  return 1;
+}
+
+}  // namespace RoboidControl

Things/Motor.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+class Motor : public Thing {
+ public:
+  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 float GetTargetVelocity();
+
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+
+ protected:
+  float targetVelocity = 0;
+};
+
+}  // namespace RoboidControl

Things/RelativeEncoder.cpp

@@ -0,0 +1,22 @@
+#include "RelativeEncoder.h"
+
+namespace RoboidControl {
+
+RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) {
+  this->type = Type::RelativeEncoder;
+}
+
+float RelativeEncoder::GetRotationSpeed() {
+  return rotationSpeed;
+}
+
+int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127);
+  return 1;
+}
+
+void RelativeEncoder::ProcessBinary(char* data) {
+  this->rotationSpeed = (float)data[0] / 127;
+}
+
+}  // namespace RoboidControl

Things/RelativeEncoder.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+/// @brief An Incremental Encoder measures the rotations of an axle using a
+/// rotary sensor. Some encoders are able to detect direction, while others can
+/// not.
+class RelativeEncoder : public Thing {
+ public:
+  /// @brief Creates a sensor which measures distance from pulses
+  /// @param pulsesPerRevolution The number of pulse edges which make a
+  /// full rotation
+  /// @param distancePerRevolution The distance a wheel travels per full
+  /// rotation
+  //RelativeEncoder(Participant* owner);
+  // RelativeEncoder(Thing* parent);
+  RelativeEncoder(Thing* parent = Thing::LocalRoot());
+
+  /// @brief Get the rotation speed 
+  /// @return The speed in revolutions per second
+  virtual float GetRotationSpeed();
+  float rotationSpeed = 0;
+
+  /// @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
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  /// @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:
+    /// @brief rotation speed in revolutions per second
+    //float _rotationSpeed;
+};
+
+}  // namespace RoboidControl

Things/TemperatureSensor.cpp

@@ -4,9 +4,9 @@
 
 namespace RoboidControl {
 
-TemperatureSensor::TemperatureSensor(Participant* participant,
+TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent) {
-                                     unsigned char thingId)
+  this->type = Type::TemperatureSensor;
-    : Thing(participant, Type::TemperatureSensor, thingId) {}
+}
 
 void TemperatureSensor::SetTemperature(float temp) {
   this->temperature = temp;

Things/TemperatureSensor.h

@@ -7,15 +7,17 @@ namespace RoboidControl {
 /// @brief A temperature sensor
 class TemperatureSensor : public Thing {
  public:
-  /// @brief The measured temperature
-  float temperature = 0;
-
   /// @brief The default constructor
   //TemperatureSensor();
   /// @brief Create a temperature sensor with the given ID
   /// @param networkId The network ID of the sensor
   /// @param thingId The ID of the thing
-  TemperatureSensor(Participant* participant, unsigned char thingId);
+  TemperatureSensor(Participant* participant); //, unsigned char thingId);
+  // TemperatureSensor(Thing* parent);
+  TemperatureSensor(Thing* parent = Thing::LocalRoot());
+
+      /// @brief The measured temperature
+      float temperature = 0;
 
   /// @brief Manually override the measured temperature
   /// @param temperature The new temperature

Things/TouchSensor.cpp

@@ -1,23 +1,36 @@
 #include "TouchSensor.h"
 
 namespace RoboidControl {
-TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
 
-TouchSensor::TouchSensor(Participant* participant)
+TouchSensor::TouchSensor(Thing* parent) : Thing(parent) {
-    : Thing(participant, Thing::Type::TouchSensor) {}
+  this->type = Type::TouchSensor;
+  this->name = "Touch sensor";
+}
 
-TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
+bool TouchSensor::IsTouching() {
-  this->SetParent(parent);
+  return this->internalTouch || this->externalTouch;
+}
+
+// void TouchSensor::PrepareForUpdate() {
+//   //this->internalTouch = this->externalTouch;
+// }
+
+void TouchSensor::Update(bool recursive) {
+  Thing::Update(recursive);
 }
 
 int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  return 0;
+  std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl;
+  bytes[(*ix)++] = this->internalTouch ? 1 : 0;
+  return 1;
 }
 
 void TouchSensor::ProcessBinary(char* bytes) {
-  // if (bytes[0] == 1)
+  this->externalTouch = (bytes[0] == 1);
-  //   std::cout << this->name << " is Touching something!\n";
+  if (this->externalTouch)
-  this->touchedSomething |= (bytes[0] == 1);
+    std::cout << "touching!\n";
+  else
+    std::cout << "not touching\n";
 }
 
 }  // namespace RoboidControl

Things/TouchSensor.h

@@ -6,29 +6,31 @@ namespace RoboidControl {
 
 /// @brief A sensor which can detect touches
 class TouchSensor : public Thing {
+// When finishing this release (0.3), I notice that this is equivalent to a digital sensor
  public:
-  /// @brief Create a touch sensor with isolated participant
+  /// @brief Create a new child touch sensor
-  TouchSensor();
+  /// @param parent The parent thing
-  /// @brief Create a touch sensor
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  TouchSensor(Participant* participant);
+  /// an ID
-  /// @brief Create a temperature sensor with the given ID
+  TouchSensor(Thing* parent = Thing::LocalRoot());
-  /// @param networkId The network ID of the sensor
-  /// @param thingId The ID of the thing
-  TouchSensor(Thing* parent);
-  // TouchSensor(RemoteParticipant* participant, unsigned char networkId,
-  // unsigned char thingId);
 
   /// @brief Value which is true when the sensor is touching something, false
   /// otherwise
-  bool touchedSomething = false;
+  bool IsTouching();
 
-  /// @brief Function to create a binary message with the temperature
+  //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
   int GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @brief Function to extract the temperature received in the binary message
+  /// @brief Function used to process binary data received for this touch sensor
-  /// @param bytes The binary data
+  /// @param bytes The binary data to process
   virtual void ProcessBinary(char* bytes) override;
+protected:
+  bool externalTouch = false;
+  bool internalTouch = false;
 };
 
 }  // namespace RoboidControl

Windows/WindowsParticipant.cpp

@@ -85,9 +85,9 @@ void ParticipantUDP::Receive() {
     unsigned int sender_port = ntohs(client_addr.sin_port);
 
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::ParticipantUDP* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    // RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port);
     // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":"
     //   //           << sender_port << "\n";
     //   // std::cout << "New remote participant " <<

examples/BB2A/main.cpp

@@ -17,32 +17,32 @@ using namespace RoboidControl;
 
 int main() {
   // The robot's propulsion is a differential drive
-  DifferentialDrive* bb2b = new DifferentialDrive();
+  DifferentialDrive bb2b = DifferentialDrive();
   // Is has a touch sensor at the front left of the roboid
-  TouchSensor* touchLeft = new TouchSensor(bb2b);
+  TouchSensor touchLeft = TouchSensor(&bb2b); 
   // and other one on the right
-  TouchSensor* touchRight = new TouchSensor(bb2b);
+  TouchSensor touchRight = TouchSensor(&bb2b);
 
   // Do forever:
   while (true) {
     // The left wheel turns forward when nothing is touched on the right side
     // and turn backward when the roboid hits something on the right
-    float leftWheelSpeed = (touchRight->touchedSomething) ? -600.0f : 600.0f;
+    float leftWheelSpeed = (touchRight.IsTouching()) ? -600.0f : 600.0f;
     // The right wheel does the same, but instead is controlled by
     // touches on the left side
-    float rightWheelSpeed = (touchLeft->touchedSomething) ? -600.0f : 600.0f;
+    float rightWheelSpeed = (touchLeft.IsTouching()) ? -600.0f : 600.0f;
     // When both sides are touching something, both wheels will turn backward
     // and the roboid will move backwards
-    bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
+    bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
 
     // Update the roboid state
-    bb2b->Update(true);
+    bb2b.Update(true);
 
     // and sleep for 100ms
 #if defined(ARDUINO)
-    delay(100);
+    delay(10);
 #else
-    sleep_for(milliseconds(100));
+    sleep_for(milliseconds(10));
 #endif
   }
 

examples/CMakeLists.txt

@@ -0,0 +1,8 @@
+# examples/CMakeLists.txt
+
+# Check if the options are enabled and add the corresponding examples
+if(BUILD_EXAMPLE_BB2A)
+    add_executable(BB2A BB2A/main.cpp)  # Adjust the path as necessary
+
+    target_link_libraries(BB2A RoboidControl)
+endif()

examples/README.md

@@ -0,0 +1 @@
+Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino

test/CMakeLists.txt

@@ -0,0 +1,30 @@
+# Unit test configuration
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    DOWNLOAD_EXTRACT_TIMESTAMP ON
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+file(GLOB_RECURSE test_srcs *.cc)
+message(STATUS "Test sources: ${test_srcs}")
+add_executable(
+    RoboidControlTest
+    ${test_srcs}
+)
+message(STATUS "RoboidControlTest target created")
+target_link_libraries(
+    RoboidControlTest
+    gtest_main
+    RoboidControl
+    LinearAlgebra
+)
+
+include(GoogleTest)
+gtest_discover_tests(RoboidControlTest)

test/participant_test.cc

@@ -4,111 +4,75 @@
 // not supported using Visual Studio 2022 compiler...
 #include <gtest/gtest.h>
 
-#include <chrono>
-#include <thread>
-// #include <ws2tcpip.h>
-
-#include "Participant.h"
 #include "Participants/SiteServer.h"
 #include "Thing.h"
 
+#include <chrono>
+#include <thread>
+
 using namespace RoboidControl;
 
-// Function to get the current time in milliseconds as unsigned long
+TEST(Participant, Participant) {
-unsigned long get_time_ms() {
+  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7682);
-  auto now = std::chrono::steady_clock::now();
+
-  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+  unsigned long milliseconds = Thing::GetTimeMs();
-      now.time_since_epoch());
+  unsigned long startTime = milliseconds;
-  return static_cast<unsigned long>(ms.count());
+  while (milliseconds < startTime + 7000) {
+    participant->Update();
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
+  }
+  SUCCEED();
 }
 
-// class RoboidControlSuite : public ::testing::test {
+TEST(Participant, SiteServer) {
-//   TEST_F(RoboidControlSuite, HiddenParticipant) {
+  SiteServer* siteServer = new SiteServer();
-//     Thing thing = Thing();
-  
-//     unsigned long milliseconds = get_time_ms();
-//     unsigned long startTime = milliseconds;
-//     while (milliseconds < startTime + 1000) {
-//       Thing.Update(milliseconds);
-  
-//       milliseconds = get_time_ms();
-//     }
-//     ASSERT_EQ(1, 1);
-//   }
-// }
 
-class ParticipantSuite : public ::testing::Test {
+  unsigned long milliseconds = Thing::GetTimeMs();
-protected:
+  unsigned long startTime = milliseconds;
-  // SetUp and TearDown can be used to set up and clean up before/after each
+  while (milliseconds < startTime + 7000) {
-  // test
+    siteServer->Update();
-  void SetUp() override {
+    
-    // Initialize test data here
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
   }
+  SUCCEED();
+}
 
-  void TearDown() override {
+TEST(Participant, SiteParticipant) {
-    // Clean up test data here
+  SiteServer* siteServer = new SiteServer(7681);
+  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
+
+  unsigned long milliseconds = Thing::GetTimeMs();
+  unsigned long startTime = milliseconds;
+  while (milliseconds < startTime + 7000) {
+    siteServer->Update();
+    participant->Update();
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
   }
-};
+  SUCCEED();
+}
 
+TEST(Participant, ThingMsg) {
+  SiteServer* siteServer = new SiteServer(7681);
+  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
+  Thing* thing = new Thing();
+  thing->SetName("First Thing");
+  thing->SetModel("https://passer.life/extras/ant.jpg");
+  
+  unsigned long milliseconds = Thing::GetTimeMs();
+  unsigned long startTime = milliseconds;
+  while (milliseconds < startTime + 7000) {
+    siteServer->Update();
+    participant->Update();
 
-
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-// TEST_F(ParticipantSuite, ParticipantUDP) {
+    milliseconds = Thing::GetTimeMs();
-//   ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681);
+  }
-
+  SUCCEED();
-//   unsigned long milliseconds = get_time_ms();
+}
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     participant->Update(milliseconds);
-
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
-
-// TEST_F(ParticipantSuite, SiteServer) {
-//   SiteServer site = SiteServer(7681);
-
-//   unsigned long milliseconds = get_time_ms();
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     site.Update(milliseconds);
-
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
-
-// TEST_F(ParticipantSuite, SiteParticipant) {
-//   SiteServer site = SiteServer(7681);
-//   ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
-
-//   unsigned long milliseconds = get_time_ms();
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     site.Update(milliseconds);
-//     participant.Update(milliseconds);
-
-//     std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
-
-// TEST_F(ParticipantSuite, Thing) {
-//   SiteServer site = SiteServer(7681);
-//   ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
-//   Thing thing = Thing(&participant);
-
-//   unsigned long milliseconds = get_time_ms();
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     site.Update(milliseconds);
-//     participant.Update(milliseconds);
-
-//     std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
 
 #endif

test/thing_test.cc

@@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 1000) {
-    Thing::UpdateThings(milliseconds);
+    thing->Update();
 
     milliseconds = Thing::GetTimeMs();
   }
@@ -23,13 +23,13 @@ TEST(RoboidControlSuite, HiddenParticipant) {
 }
 
 TEST(RoboidControlSuite, IsolatedParticipant) {
-  ParticipantUDP* participant = ParticipantUDP::Isolated();
+  ParticipantUDP* participant = new ParticipantUDP(0);
-  Thing* thing = new Thing(participant);
+  Thing* thing = new Thing();
 
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 1000) {
-    participant->Update(milliseconds);
+    participant->Update();
 
     milliseconds = Thing::GetTimeMs();
   }