Cleanup, but still crashes after about 20-30 secs.

8b6f628 Fix Matrix unit test
28a3064 Fix tests
91f4280 Reduced the use of raw pointers
83539df Fixes to make it work againg, but failing at udp.begin()
3cca327 Optimizations
361807c Performance improvements
7bd83fa Processomg Acc data * propagating
0eb2851 Linking works
06ff5e9 Added ESP-IDF Wifi/UDP support (but it still give linker errors)
7461a1f Implemented integrate function
b7f102a Fix matrix2 tests
7d2dd08 First steps implementation Matrix operations
f05b411 Cleanup
05a3b09 Update namespace, Windows compatibility
95a248a First udp communcation is working
fea15c3 removed namespace Passer
753de2d Fixes
9674f68 Disabled test failing on MacOS
5b89234 Made it work on MacOS
d0ba29e Updated LinearAlgebra
ebfb4fb Merge commit '0b63a044eb84175cc922f3f1fe0ce039d101bf7b'
0b63a04 Updated documentation, cleanup
a0082bd Add direction documentation
770ce32 Merge commit '2b5f5cd175b20fa01e04b696dfe0cfbe2c4ad9da'
2b5f5cd Removed shorthand Deg/Rad aliases... (see below)
d212db2 Trying to fix specialization before instantiation error
e445e5b Trying to fix specialization before instantiation error
f47e504 Trying to fix specialization before instantiation error
b877d4d Trying to fix specialization before instantiation error
06e8623 Trying to fix specialization before instantiation error
e97aee9 Trying to fix specialization before instantiation error
0e00e5d Merge branch 'main' of http://gitlab.passervr.com/passer/cpp/linear-algebra
52de55d Trying to fix specialization before instantiation error
47ba024 Trying to fix specialization before instantiation error
2402030 Trying to fix specialization before instantiation error
4e3f253 Fixed gitignore
06c70e2 Merge branch 'Experimental'
6f30334 Updated PolarOf, removed default Polar type
94a3105 Cleanup
89a5711 Fixed typo in documentation
64b7f11 Removed default Angle type
58beb36 Completed AngleOf documentation
5f58a2c Updated documentation
97d937e Fixed (copilot) unit tests
585a3b0 Removed default Spherical type
d3226fd Added copilot test documentation
a7aa679 Added a performance test
2c57c3f Fixed direction for spherical
28ee225 Normalize Spherical at creation
800eb75 improved rounding of discrete angles
536d6ce Add Binary support
18756ba Correct axis on quaternion from swingtwist
06e42c5 Normalized swing twist
edbb4b1 Removes Angle::deg90/180 because of issues
0a07b23 SwingTwist == and Angle
9406b57 SwingTwist::AngleAxis using Direction
06da9e5 Removed AngleAxis, Sperhical will be used instead
2e61e1b Fix unit test
54d0318 Extended unit tests (plus fixes)
8286c1c Minor improvements
95a6fb3 Improved Angle quality
9eca318 Fixed inverse
0ebfce4 Spherical direction
136e44e Fixed unit tests
fb5cee8 Proper angle implementation (unit tests still fail)
92d5ef0 Normalizing directions
1ea65d5 Made toQuestion const
f1c70c7 Fix unit tests
2ad6a5a Fix include
11259a9 Extend functionality
3363388 Extended AngleAxis & Direction
09e25a8 Add Asin and Atan
a646e93 Add ACos
407e771 Add WithDistance
69bad8f Added degrees, radians, cos, sin, and tan functions
5c3dfa6 Adde degrees/readians and poc ACos
05c61a3 Add distance between
6a1d043 Added Angle comparison
6b3bcfc Another anglebetween fix
b5a6330 Fix anglebetween
b975aed Added angle and rotate functions
f483439 Moved Polar/Spherical to template class completely
e0ef43a Merge branch 'Experimental' of http://gitlab.passervr.com/passer/cpp/linear-algebra into Experimental
b460bec Added scaling to SphericalOf<T>
e10e010 Added Direction to library sources
dedaa31 Fix unit tests
1da93ca Fix ToVector3
9c3503f Added SphericalOf
353cb1b Add conversion from Quaternion
cf86ba8 Cleanup template classes
ea6894e Fix generic template functions
1ce7234 Bug fixes
38ebb34 Added Direction to replace Axis
c72bba4 Change Vector3::Angle return type to AngleOf<float>
18cf653 Added scaling support
51772a1 Added add/subtract for discrete angles
49c6740 Make Angle -> float conversion explicit
608c45c Fix precision error
4591aef Added Spherical16
48828e2 Fix int/float issues
86b7c21 Added Degrees inline function
a4b0491 Fix unit tests
b81b77b Extend AngleOf support
c70c079 Add spherical subtract
2bad384 Added spherical addition
a25a8be Fixed namespace issues
f8009a7 Updated namespace
91c7b20 Fix gitlab test runner
78468e3 Renamed VectorAlgebra project to LinearAlgebra
47a6368 Fix Vector3
ee62cba XYZ deprecation
62d7439 Extend ulitity functions
95713c8 Cleanup, added utility functions
f7d9d97 Cleanup
66e47d7 Cleanup Vector2 and Polar
e922a01 fix == operator
5693097 Initial subtractop test
c411b43 Textual improvement
0c54276 equality support
b1e34b6 Improved unit tests
6ddb074 Improved unit tests
5489b3c Fix test
3d971c1 Improve namespace
791bd78 namespace improvement
9bea94f Use Angle type for Polar
8a2ce21 Swapped polar constructor param order
e5f8d73 namespace fix
8e7a8c6 Fix namespaces
64ca768 normalize toAngleAxis result
4385bef Add angle-axis
4b07328 Add conversion from Vector3
fe12c99 Fix Spherical reference
c274c3e Add conversion from Spherical
66ff721 Fixed wrong conversion to short
264bcbc Added 32 bit angle
f190dd7 Merge branch 'DiscreteAngle' into Experimental
ae55a24 Further  vector3 conversion fixes
ec0cc47 Fix spherical to vector
89b5da8 Fix ambiguity
e62fa96 Fix specialization again
430344a Fix specialization error
3e5ede0 Removed old file
e62bacc First Discrete Angle functions
87d2c11 Set strict warnings
5141766 Add spherical.toVector3 test (and fixes)
8e3b9e2 Fix Polar test
7b85556 Addedfirst Polar test
395f82d Fix Spherical Unit test
0cbef79 Add first Spherical Unit test
dc601a1 Removed Vector2.tofactor
159bdae Added spherical.tovector (but is is buggy)
273ebae Fixes
0468031 Bug fix
3714507 Implemented templated angles
69e7ee1 Added DiscreteAgle
2cbf259 Make negation virtual override
2b50bab Add negation for DiscreteAngle
de3a647 Updated negation
254bb27 Added negation
bd04285 Make superclass accessable
2bb0009 Initial implementation

git-subtree-dir: LinearAlgebra
git-subtree-split: 8b6f628d0c6b9e72260ce1d9c0437124c2d7b6d8

Arduino/ArduinoParticipant.cpp

@@ -1,4 +1,9 @@
 #include "ArduinoParticipant.h"
+#if defined(ARDUINO)
+
+#if !defined(NO_STD)
+#include <iostream>
+#endif
 
 #if defined(ARDUINO)
 #if defined(ARDUINO_ARCH_ESP8266)
@@ -17,19 +22,20 @@
 #endif
 
 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,55 +65,51 @@ 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
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
   // << 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";
@@ -122,5 +131,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   return true;
 };
 
-}  // namespace Arduino
 }  // namespace RoboidControl
+#endif

Arduino/ArduinoParticipant.h

@@ -1,32 +1,22 @@
 #pragma once
+#if defined(ARDUINO)
 
 #include "Participants/ParticipantUDP.h"
 
-#if defined(HAS_WIFI)
-#include <WiFiUdp.h>
-#endif
-
 namespace RoboidControl {
-namespace Arduino {
 
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
  public:
-  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Setup();
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(RemoteParticipantUDP* 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();
 };
 
-}  // namespace Arduino
 }  // namespace RoboidControl
+#endif

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;
+
+ParticipantUDPGeneric* 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 ParticipantUDPGeneric("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

EspIdf/EspIdfParticipant.cpp

@@ -4,13 +4,12 @@
 #include "esp_wifi.h"
 #endif
 
+#include <arpa/inet.h>
 namespace RoboidControl {
-namespace EspIdf {
 
-void ParticipantUDP::Setup(int localPort,
+void ParticipantUDP::SetupUDP(int localPort,
                            const char* remoteIpAddress,
                            int remotePort) {
-#if defined(IDF_VER)
   std::cout << "Set up UDP\n";
   GetBroadcastAddress();
 
@@ -22,42 +21,57 @@ void ParticipantUDP::Setup(int localPort,
   }
 
   // Create a UDP socket
-  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-  if (this->sockfd < 0) {
+  if (this->sock < 0) {
     std::cout << "Unable to create UDP socket: errno " << errno << "\n";
     vTaskDelete(NULL);
     return;
   }
 
-  // Set up the server address structure
+  /*
+  // Set up the receiving(local) address
   struct sockaddr_in local_addr;
   memset(&local_addr, 0, sizeof(local_addr));
   local_addr.sin_family = AF_INET;
-  local_addr.sin_port = htons(this->port);
+  local_addr.sin_port = htons(localPort);
   local_addr.sin_addr.s_addr =
       htonl(INADDR_ANY);  // Listen on all available network interfaces
 
-  // Bind the socket to the address and port
+  // Bind the socket to the receiving address
-  if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) <
+  if (bind(this->sock, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) {
-      0) {
     std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
-    close(sockfd);
+    close(sock);
     vTaskDelete(NULL);
     return;
   }
+    */
 
-  // Initialize the dest_addr structure
+  // Initialize the destination(remote) address
   memset(&this->dest_addr, 0,
          sizeof(this->dest_addr));  // Clear the entire structure
   this->dest_addr.sin_family = AF_INET;
   this->dest_addr.sin_port = htons(this->remoteSite->port);
-  inet_pton(AF_INET, this->remoteSite->ipAddress,
+  this->dest_addr.sin_addr.s_addr = inet_addr(this->remoteSite->ipAddress);
-            &this->dest_addr.sin_addr.s_addr);
+  // inet_pton(AF_INET, this->remoteSite->ipAddress,
+  //           &this->dest_addr.sin_addr.s_addr);
 
-  std::cout << "Wifi sync started local " << this->port << ", remote "
+    this->connected = true;
+
+  std::cout << "Wifi sync started local " << localPort << ", remote "
             << this->remoteSite->ipAddress << ":" << this->remoteSite->port
             << "\n";
-#endif  // IDF_VER
+
+  // std::cout << "socket: " << (int)this->sock << std::endl;
+  // ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+  // int bufferSize = msg->Serialize(this->buffer);
+  // int err = sendto(this->sock, buffer, bufferSize, 0,
+  //                  (struct sockaddr*)&dest_addr, sizeof(dest_addr));
+  // if (errno != 0)
+  //   std::cout << "AASend error " << err << " or " << errno << "\n";
+  // else
+  //   std::cout << "AASend SUCCESS\n";
+
+  //SendTest();
 }
 
 void ParticipantUDP::GetBroadcastAddress() {
@@ -83,10 +97,11 @@ void ParticipantUDP::GetBroadcastAddress() {
 #endif  // IDF_VER
 }
 
-void ParticipantUDP::Receive() {
+void ParticipantUDP::ReceiveUDP() {
 #if defined(IDF_VER)
+/*
   struct pollfd fds[1];
-  fds[0].fd = sockfd;
+  fds[0].fd = sock;
   fds[0].events = POLLIN;  // We're looking for data available to read
 
   // Use poll() with a timeout of 0 to return immediately
@@ -103,7 +118,7 @@ void ParticipantUDP::Receive() {
   char sender_ipAddress[INET_ADDRSTRLEN];
 
   while (ret > 0 && fds[0].revents & POLLIN) {
-    int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
+    int packetSize = recvfrom(this->sock, buffer, sizeof(buffer) - 1, 0,
                               (struct sockaddr*)&source_addr, &addr_len);
     if (packetSize < 0) {
       std::cout << "recvfrom() error\n";
@@ -126,18 +141,66 @@ void ParticipantUDP::Receive() {
     }
   }
   // std::cout << "no more messages\n";
-
+*/
 #endif  // IDF_VER
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
-#if defined(IDF_VER)
-  // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
-  //           << remoteParticipant->port << "\n";
 
-  int err = sendto(this->sockfd, buffer, bufferSize, 0,
+ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
-                   (struct sockaddr*)&dest_addr, sizeof(dest_addr));
+    : ParticipantUDPGeneric(ipAddress, port, localPort) {}
-  if (errno != 0)
+
+// bool ParticipantUDP::SendTest() {
+// #if defined(IDF_VER)
+//   // std::cout << "socket: " << (int)this->sock << std::endl;
+//   // UBaseType_t stack_size = uxTaskGetStackHighWaterMark(NULL);  // NULL to check the main task
+//   // size_t free_heap = xPortGetFreeHeapSize();
+//   // std::cout <<  "Stack High Water Mark: " << stack_size << " heap " << free_heap << std::endl;
+
+//   ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+//   int bSize = msg->Serialize(this->buffer);
+//   // std::cout << "buffer size " << bSize << std::endl;
+//   int err = sendto(this->sock, buffer, bSize, 0, (struct sockaddr*)&dest_addr,
+//                    sizeof(dest_addr));
+//   if (errno != 0)
+//     std::cout << "BBSend error " << err << " or " << errno << "\n";
+//   else
+//     std::cout << "BBSend SUCCESS\n";
+
+// #endif
+//   return true;
+// }
+
+bool ParticipantUDP::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->remoteSite->ipAddress << std::endl;
+
+  return this->SendTo(this->remoteSite, bufferSize);
+}
+
+bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
+                            int bufferSize) {
+#if defined(IDF_VER)
+  uint16_t port = ntohs(dest_addr.sin_port);
+
+  char ip_str[INET_ADDRSTRLEN];
+  inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str));
+  std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n";
+
+  // Print the IP address and port
+  // printf("IP Address: %s\n", ip_str);
+  // printf("Port: %d\n", port);
+
+  this->dest_addr.sin_port = htons(remoteParticipant->port);
+  this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
+
+  int err = sendto(this->sock, buffer, bufferSize, 0,
+                   (struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr));
+  if (errno < 0)
     std::cout << "Send error " << err << " or " << errno << "\n";
 
 #endif
@@ -154,7 +217,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   dest_addr.sin_family = AF_INET;
   dest_addr.sin_port = htons(this->port);
   inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
-  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
+  int err = sendto(sock, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
                    sizeof(dest_addr));
   if (err != 0)
     std::cout << "Publish error\n";
@@ -162,5 +225,4 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   return true;
 };
 
-}  // namespace EspIdf
 }  // namespace RoboidControl

EspIdf/EspIdfParticipant.h

@@ -1,26 +1,47 @@
 #pragma once
+#if defined(IDF_VER)
 
 #include "Participants/ParticipantUDP.h"
 
-#if defined(IDF_VER)
 #include "lwip/sockets.h"
-#endif
 
 namespace RoboidControl {
-namespace EspIdf {
 
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
  public:
+  /// @brief Create a participant without connecting to a site
+  /// @param port The port on which the participant communicates
+  /// These participant typically broadcast Participant messages to let site
+  /// servers on the local network know their presence. Alternatively they can
+  /// broadcast information which can be used directly by other participants.
+  ParticipantUDP(int port = 7681);
+  /// @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
+  /// @param localPort The port used by the local participant
+  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
+
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
+  // bool SendTest();
+
+  /// @brief Sens a message to the remote site (if set)
+  /// @param msg The message to send
+  /// @return True if a message could be sent.
+  bool Send(IMessage* msg) override;
+  void SetupUDP(int localPort,
+                const char* remoteIpAddress,
+                int remotePort) override;
+  void ReceiveUDP() override;
+
  protected:
 #if defined(IDF_VER)
   char broadcastIpAddress[INET_ADDRSTRLEN];
 
-  int sockfd;
+  int sock;
   struct sockaddr_in dest_addr;
   // struct sockaddr_in src_addr;
 #endif
@@ -28,5 +49,6 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
   void GetBroadcastAddress();
 };
 
-}  // namespace EspIdf
 }  // namespace RoboidControl
+
+#endif

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>());
@@ -166,3 +168,5 @@ void SwingTwistOf<T>::Normalize() {
 
 template class SwingTwistOf<float>;
 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,32 @@
 #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
 
+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;
@@ -22,25 +42,62 @@ Participant::Participant(const char* ipAddress, int port) {
   this->ipAddress = addressString;
   this->port = port;
 }
-
+*/
 Participant::~Participant() {
-  delete[] this->ipAddress;
+  // registry.Remove(this);
+  // delete[] this->ipAddress;
 }
 
-void Participant::Update(unsigned long currentTimeMs) {
+void Participant::Update(bool recurse) {
   for (Thing* thing : this->things) {
     if (thing != nullptr)
-      thing->Update(currentTimeMs, true);
+      thing->Update();
   }
 }
 
-Thing* Participant::Get(unsigned char thingId) {
+bool Participant::Send(IMessage* msg) {
+    std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
+            << " to base Participant without communcation support " << std::endl;
+
+  return 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->SendTo(this, bufferSize);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  return thisPosix->SendTo(this, bufferSize);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  return thisArduino->SendTo(this, bufferSize);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  return thisEspIdf->SendTo(this, bufferSize);
+#else
+  return false;
+#endif
+}
+*/
+
+Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
   for (Thing* thing : this->things) {
-    if (thing->id == thingId)
+    if (thing->owner->networkId == networkId && 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,24 +108,35 @@ 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 << ":"
+    std::cout << "Add thing with generated ID " 
-    //           << this->port << "[" << (int)thing->id << "]\n";
+    //<< this->ipAddress << ":" << this->port
+    << "[" << (int)thing->id << "]\n";
   } else {
-    Thing* foundThing = Get(thing->id);
+    Thing* foundThing = Get(thing->owner->networkId, thing->id);
     if (foundThing == nullptr) {
 #if defined(NO_STD)
       this->things[this->thingCount++] = thing;
 #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";
+      "["
+                << (int)thing->id << "]\n";
     } else {
-      // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
+      std::cout << "Did not add, existing thing " 
-      //           << this->port << "[" << (int)thing->id << "]\n";
+      //<< this->ipAddress << ":" << this->port
+       << "[" << (int)thing->id << "]\n";
     }
   }
 }
@@ -89,22 +157,95 @@ 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
+// /*
-//       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
+// Participant* ParticipantRegistry::Get(const char* ipAddress,
-//       //           << "\n";
+//                                       unsigned int port) {
-//       thing->Update(currentTimeMs);
+// #if !defined(NO_STD)
+//   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->networkId);
+//       //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,61 @@
 #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 +63,86 @@ 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 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 networkId The network ID of 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 networkId, unsigned char thingId);
-  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(bool recurse = true);
+
+#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/IsolatedParticipant.cpp

@@ -1,14 +1,16 @@
+/*
 #include "IsolatedParticipant.h"
 #include "ParticipantUDP.h"
 
 namespace RoboidControl {
 
-static ParticipantUDP* isolatedParticipant = nullptr;
+static ParticipantUDPGeneric* isolatedParticipant = nullptr;
 
 Participant* IsolatedParticipant::Isolated() {
   if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDP(0);
+    isolatedParticipant = new ParticipantUDPGeneric(0);
   return isolatedParticipant;
 }
 
 }  // namespace RoboidControl
+  */

Participants/IsolatedParticipant.h

@@ -1,3 +1,4 @@
+/*
 #include "Participant.h"
 
 namespace RoboidControl {
@@ -11,3 +12,4 @@ class IsolatedParticipant {
 };
 
 }
+*/

Participants/ParticipantUDP.cpp

@@ -1,86 +1,184 @@
 #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 "Things/DifferentialDrive.h"
+#include "Things/DistanceSensor.h"
+#include "Things/TouchSensor.h"
 
 #include <string.h>
 
 namespace RoboidControl {
 
-ParticipantUDP::ParticipantUDP(int port) {
+#pragma region ParticipantRegistry
-  this->ipAddress = "0.0.0.0";
+
+ParticipantRegistry ParticipantUDPGeneric::registry;
+
+RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
+                                               unsigned int port) {
+#if !defined(NO_STD)
+  for (RemoteParticipantUDP* 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;
+}
+
+RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
+#if !defined(NO_STD)
+  for (RemoteParticipantUDP* 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;
+}
+
+RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
+                                               unsigned int port) {
+  RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
+  Add(participant);
+  return participant;
+}
+
+void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
+  Participant* foundParticipant = Get(participant->networkId);
+  // 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(RemoteParticipantUDP* participant) {
+  // participants.remove(participant);
+}
+
+#if defined(NO_STD)
+RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#else
+const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#endif
+
+#pragma endregion ParticipantRegistry
+
+RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
+  // make a copy of the ip address string
+  int addressLength = (int)strlen(ipAddress);
+  int stringLength = addressLength + 1;
+  char* addressString = new char[stringLength];
+#if defined(_WIN32) || defined(_WIN64)
+  strncpy_s(addressString, stringLength, ipAddress,
+            addressLength);  // Leave space for null terminator
+#else
+  strncpy(addressString, ipAddress, addressLength);
+#endif
+  addressString[addressLength] = '\0';
+
+  this->ipAddress = addressString;
   this->port = port;
+}
+
+bool RemoteParticipantUDP::Send(IMessage* msg) {
+  // No message is actually sent, because this class has no networking
+  // implementation
+  return false;
+}
+
+#pragma region Init
+
+ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
+    : RemoteParticipantUDP("127.0.0.1", port) {
+  this->name = "ParticipantUDP";
   this->remoteSite = nullptr;
   if (this->port == 0)
     this->isIsolated = true;
+  registry.Add(this);
+
+  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  std::cout << "P2 " << (int)this->root << std::endl;
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
 }
 
-ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
+ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
-    : Participant("127.0.0.1", localPort) {
+                                             int port,
+                                             int localPort)
+    : RemoteParticipantUDP("127.0.0.1", localPort) {
+  this->name = "ParticipantUDP";
   if (this->port == 0)
     this->isIsolated = true;
   else
-    this->remoteSite = new Participant(ipAddress, port);
+    this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
+  registry.Add(this);
+
+  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  std::cout << "P1 " << (int)this->root << std::endl;
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
 }
 
-static ParticipantUDP* isolatedParticipant = nullptr;
+void ParticipantUDPGeneric::begin() {
-
+  if (this->isIsolated || this->remoteSite == nullptr)
-ParticipantUDP* ParticipantUDP::Isolated() {
-  if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDP(0);
-  return isolatedParticipant;
-}
-
-void ParticipantUDP::begin() {
-  if (this->isIsolated)
     return;
 
   SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
 }
 
-void ParticipantUDP::SetupUDP(int localPort,
+#pragma endregion Init
-                              const char* remoteIpAddress,
-                              int remotePort) {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  thisWindows->Setup(localPort, remoteIpAddress, remotePort);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
-#endif
-  this->connected = true;
-}
 
-void ParticipantUDP::Update(unsigned long currentTimeMs) {
+#pragma region Update
-  if (currentTimeMs == 0)
+
-    currentTimeMs = Thing::GetTimeMs();
+// The update order
+// 1. receive external messages
+// 2. update the state
+// 3. send out the updated messages
+void ParticipantUDPGeneric::Update(bool recurse) {
+  unsigned long currentTimeMs = Thing::GetTimeMs();
 
   if (this->isIsolated == false) {
     if (this->connected == false)
@@ -88,122 +186,101 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
 
     if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
       ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+
       if (this->remoteSite == nullptr)
         this->Publish(msg);
       else
-        this->Send(this->remoteSite, msg);
+        this->Send(msg);
-      delete msg;
 
+      delete msg;
       this->nextPublishMe = currentTimeMs + this->publishInterval;
     }
 
-    this->ReceiveUDP();
+    //this->ReceiveUDP();
   }
 
+  UpdateMyThings();
+  UpdateOtherThings();
+}
+
+void ParticipantUDPGeneric::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 ParticipantUDPGeneric::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 : 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,
+void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant,
-                                   Thing* thing) {
+                                          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 ParticipantUDPGeneric::Send(IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
+  if (this->remoteSite != nullptr)
-  if (bufferSize <= 0)
+    return this->remoteSite->Send(msg);
-    return true;
 
-#if defined(_WIN32) || defined(_WIN64)
+  return true;
-  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) {
+void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
   // std::cout << "Publish thing info" << thing->networkId << "\n";
   //  Strange, when publishing, the network id is irrelevant, because it is
   //  connected to a specific site...
@@ -224,47 +301,31 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) {
   delete customMsg;
 }
 
-bool ParticipantUDP::Publish(IMessage* msg) {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  return thisWindows->Publish(msg);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  return thisPosix->Publish(msg);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  return thisArduino->Publish(msg);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->Publish(msg);
-#else
-  return false;
-#endif
-}
-
 // Send
 #pragma endregion
 
 #pragma region Receive
 
-void ParticipantUDP::ReceiveData(unsigned char packetSize,
+void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
-                                 char* senderIpAddress,
+                                        char* senderIpAddress,
-                                 unsigned int senderPort) {
+                                        unsigned int senderPort) {
-  Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
+  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
+  //           << std::endl;
+  RemoteParticipantUDP* sender =
+      this->registry.Get(senderIpAddress, senderPort);
   if (sender == nullptr) {
-    sender = this->AddParticipant(senderIpAddress, senderPort);
+    sender = this->registry.Add(senderIpAddress, senderPort);
-    std::cout << "New remote participant " << sender->ipAddress << ":"
+#if !defined(NO_STD)
-              << sender->port << std::endl;
+    // std::cout << "New remote participant " << sender->ipAddress << ":"
+    //           << sender->port << std::endl;
+#endif
   }
 
   ReceiveData(packetSize, sender);
 }
 
-void ParticipantUDP::ReceiveData(unsigned char bufferSize,
+void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
-                                 Participant* sender) {
+                                        RemoteParticipantUDP* sender) {
   unsigned char msgId = this->buffer[0];
   // std::cout << "receive msg " << (int)msgId << "\n";
   // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
@@ -275,8 +336,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,56 +377,113 @@ 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) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    ParticipantMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
             << "\n";
 #endif
 }
 
-void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* 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);
   }
 }
 
-void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    InvestigateMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "]\n";
 #endif
 }
 
-void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    ThingMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
             << (int)msg->parentId << "\n";
 #endif
+  RemoteParticipantUDP* owner = registry.Get(msg->networkId);
+  if (owner == nullptr) {
+    owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
+    owner->networkId = msg->networkId;
+    registry.Add(owner);
+  }
+
+  Thing* thing = owner->Get(msg->networkId, msg->thingId);
+  if (thing == nullptr) {
+    bool isRemote = (sender->networkId != owner->networkId);
+    thing = ProcessNewThing(owner, msg, isRemote);
+    thing->id = msg->thingId;
+    thing->type = msg->thingType;
+    thing->isRemote = isRemote;
+  }
+
+  if (msg->parentId != 0) {
+    thing->SetParent(owner->Get(msg->networkId, msg->parentId));
+    if (thing->GetParent() == nullptr)
+      std::cout << "Could not find parent" << std::endl;
+  } else
+    thing->SetParent(nullptr);
 }
 
-void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
+Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
+                                              ThingMsg* msg,
+                                              bool isRemote) {
+  switch (msg->thingType) {
+    case Thing::Type::DistanceSensor:
+      return new DistanceSensor(owner->root);
+    case Thing::Type::TouchSensor:
+      return new TouchSensor(owner->root);
+    case Thing::Type::DifferentialDrive:
+      return new DifferentialDrive(owner->root);
+    default:
+      return new Thing(owner->root);
+  }
+}
+
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    NameMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
             << (int)msg->thingId << "] ";
 #endif
 
-  Thing* thing = sender->Get(msg->thingId);
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr) {
     int nameLength = msg->nameLength;
     int stringLen = nameLength + 1;
@@ -380,41 +498,90 @@ 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) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    ModelUrlMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "]\n";
 #endif
 }
 
-void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
-#if defined(DEBUG)
+                                    PoseMsg* msg) {
+#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 = registry.Get(msg->networkId);
+  if (owner == nullptr)
+    return;
+
+  Thing* thing = owner->Get(msg->networkId, 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 ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    BinaryMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "]\n";
+#endif
+
+  Participant* owner = registry.Get(msg->networkId);
+  if (owner != nullptr) {
+    Thing* thing = owner->Get(msg->networkId, 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 ParticipantUDPGeneric::Process(RemoteParticipantUDP* 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, BinaryMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    DestroyMsg* msg) {
 #if defined(DEBUG)
-  std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
+  std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
-            << "/" << (int)msg->thingId << "] ";
+            << (int)msg->thingId << "]\n";
 #endif
 
-  Thing* thing = sender->Get(msg->thingId);
+  Thing* thing = sender->Get(msg->networkId, 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,12 +1,14 @@
 #pragma once
 
 #include "Messages/BinaryMsg.h"
+#include "Messages/DestroyMsg.h"
 #include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
+#include "Messages/NetworkIdMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
-#include "Messages/SiteMsg.h"
+#include "Messages/TextMsg.h"
 #include "Messages/ThingMsg.h"
 #include "Participant.h"
 
@@ -29,7 +31,76 @@ namespace RoboidControl {
 
 constexpr int MAX_SENDER_COUNT = 256;
 
-/// @brief A local participant is the local device which can communicate with
+class RemoteParticipantUDP : public Participant {
+ public:
+ /// @brief Create a new participant with the given communcation info
+  /// @param ipAddress The IP address of the participant
+  /// @param port The UDP port of the participant
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  RemoteParticipantUDP(const char* ipAddress, int port);
+
+  /// @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;
+
+  bool Send(IMessage* msg) override;
+};
+
+/// @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
+  RemoteParticipantUDP* 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
+  RemoteParticipantUDP* 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
+  RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
+
+  /// @brief Add a participant
+  /// @param participant The participant to add
+  void Add(RemoteParticipantUDP* participant);
+
+  /// @brief Remove a participant
+  /// @param participant The participant to remove
+  void Remove(RemoteParticipantUDP* participant);
+
+ private:
+#if defined(NO_STD)
+ public:
+  RemoteParticipantUDP** GetAll() const;
+  int count = 0;
+
+ private:
+  RemoteParticipantUDP** participants;
+#else
+ public:
+  /// @brief Get all participants
+  /// @return All participants
+  const std::list<RemoteParticipantUDP*>& GetAll() const;
+
+ private:
+  /// @brief The list of known participants
+  std::list<RemoteParticipantUDP*> participants;
+#endif
+};
+
+/// @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
@@ -40,52 +111,42 @@ constexpr int MAX_SENDER_COUNT = 256;
 /// participant is created which can be obtained using
 /// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
-class ParticipantUDP : public Participant {
+class ParticipantUDPGeneric : public RemoteParticipantUDP {
+#pragma region Init
+
  public:
   /// @brief Create a participant without connecting to a site
   /// @param port The port on which the participant communicates
   /// These participant typically broadcast Participant messages to let site
   /// servers on the local network know their presence. Alternatively they can
   /// broadcast information which can be used directly by other participants.
-  ParticipantUDP(int port = 7681);
+  ParticipantUDPGeneric(int port = 7681);
   /// @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,
+  ParticipantUDPGeneric(const char* ipAddress,
-                   int localPort = 7681);
+                        int port = 7681,
-  // Note to self: one cannot specify the port used by the local participant
+                        int localPort = 7681);
-  // 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
+  RemoteParticipantUDP* 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";
+#if !defined(ARDUINO)
-
-  // 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(__unix__) || defined(__APPLE__)
   int sock;
 #elif defined(_WIN32) || defined(_WIN64)
@@ -93,81 +154,73 @@ 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
 
-  void SendThingInfo(Participant* remoteParticipant, Thing* thing);
+#pragma region Update
-  void PublishThingInfo(Thing* thing);
 
-  bool Send(Participant* remoteParticipant, IMessage* msg);
+ public:
-  bool Publish(IMessage* msg);
+  virtual void Update(bool recurse = true) override;
-
-  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];
+  /// @brief Prepare the local things for the next update
+  // virtual void PrepMyThings();
+  virtual void UpdateMyThings();
+  virtual void UpdateOtherThings();
 
-  void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
+#pragma endregion Update
 
-  Participant* GetParticipant(const char* ipAddress, int port);
+#pragma region Send
-  Participant* AddParticipant(const char* ipAddress, int port);
 
-  void ReceiveUDP();
+  void SendThingInfo(Participant* remoteParticipant, Thing* thing);
+  void PublishThingInfo(Thing* thing);
 
-  virtual void Process(Participant* sender, ParticipantMsg* msg);
+  virtual bool Send(IMessage* msg) override;
-  virtual void Process(Participant* sender, SiteMsg* msg);
+  virtual bool Publish(IMessage* msg) = 0;
-  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);
 
-#if !defined(NO_STD)
+#pragma endregion Send
-//  public:
-//   using ThingConstructor = std::function<Thing*(Participant* participant,
-//                                                 unsigned char networkId,
-//                                                 unsigned char thingId)>;
 
-//   template <typename ThingClass>
+#pragma region Receive
-//   void Register(unsigned char thingType) {
-//     thingMsgProcessors[thingType] = [](Participant* participant,
-//                                        unsigned char networkId,
-//                                        unsigned char thingId) {
-//       return new ThingClass(participant, networkId, thingId);
-//     };
-//   };
 
-//   template <typename ThingClass>
+ protected:
-//   void Register2(unsigned char thingType, ThingConstructor f) {
+  void ReceiveData(unsigned char bufferSize,
-//     thingMsgProcessors[thingType] = [f](Participant* participant,
+                   char* senderIpAddress,
-//                                        unsigned char networkId,
+                   unsigned int senderPort);
-//                                        unsigned char thingId) {
+  void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);
-//       return f(participant, networkId, thingId);
-//     };
-//   };
 
-//  protected:
+  virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;
-//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
+
+  virtual void ReceiveUDP() = 0;
+
+  virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
+
+  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
+  virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
+                                 ThingMsg* msg,
+                                 bool isRemote);
+
+  virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg);
+
+#pragma endregion Receive
+
+public:
+  static ParticipantRegistry registry;
 
-#endif
 };
 
 }  // namespace RoboidControl
+
+#include "EspIdf/EspIdfParticipant.h"

Participants/SiteServer.cpp

@@ -9,58 +9,89 @@
 
 namespace RoboidControl {
 
-SiteServer::SiteServer(int port) {
+#pragma region Init
+
+SiteServer::SiteServer(int port) : ParticipantUDPGeneric(port) {
   this->name = "Site Server";
   this->publishInterval = 0;
 
-  this->ipAddress = "0.0.0.0";
+  //SetupUDP(port, ipAddress, 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
 }
 
-void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
+#pragma endregion Init
-  if (msg->networkId == 0) {
+
+#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 : 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(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
+  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(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
 
-void SiteServer::Process(Participant* sender, ThingMsg* msg) {
+void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
-  Thing* thing = sender->Get(msg->thingId);
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
   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* newThing;
-    //     if (thingMsgProcessor != thingMsgProcessors.end())  // found item
-    //       //newThing =
-    //           thingMsgProcessor->second(sender, msg->networkId,
-    //           msg->thingId);
-    //     else
-    //       //newThing =
-    //         new Thing(sender, msg->networkId, msg->thingId,
-    //                            (Thing::Type)msg->thingType);
-    // #endif
   }
+  thing->id = msg->thingId;
+
+  if (msg->parentId != 0) {
+    thing->SetParent(Get(msg->networkId, msg->parentId));
+    if (thing->IsRoot())
+    // if (thing->GetParent() != nullptr)
+#if defined(NO_STD)
+      ;
+#else
+      std::cout << "Could not find parent [" << (int)msg->networkId << "/"
+                << (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 {
+class SiteServer : public ParticipantUDPGeneric {
+  
+#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(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
-  virtual void Process(Participant* sender, SiteMsg* msg) override;
+  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
-  virtual void Process(Participant* sender, ThingMsg* msg) override;
+  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;
+
+#pragma endregion Receive
 
-// #if !defined(NO_STD)
-//   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

@@ -1,4 +1,5 @@
 #include "PosixParticipant.h"
+#if defined(__unix__) || defined(__APPLE__)
 
 #if defined(__unix__) || defined(__APPLE__)
 #include <arpa/inet.h>
@@ -9,9 +10,8 @@
 #endif
 
 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::SendTo(RemoteParticipantUDP* 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)
@@ -132,5 +132,6 @@ bool Publish(IMessage* msg) {
   return true;
 }
 
-}  // namespace Posix
 }  // namespace RoboidControl
+
+#endif

Posix/PosixParticipant.h

@@ -1,17 +1,24 @@
 #pragma once
+#if defined(__unix__) || defined(__APPLE__)
 
 #include "Participants/ParticipantUDP.h"
 
 namespace RoboidControl {
-namespace Posix {
 
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(RemoteParticipantUDP* 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
 }  // namespace RoboidControl
+#endif

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

@@ -10,7 +10,7 @@
 namespace RoboidControl {
 
 class Participant;
-class ParticipantUDP;
+class ParticipantUDPGeneric;
 
 #define THING_STORE_SIZE 256
 // IMPORTANT: values higher than 256 will need to change the Thing::id type
@@ -20,71 +20,122 @@ 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 Is this a remote thing?
-  /// @param name Rhe name of the thing
+  /// A remote thing is owned by other participant
-  /// @return The found thing or nullptr when nothing is found
+  /// and is not simulated by the local participant
-  Thing* FindThing(const char* name);
+  bool isRemote = false;
 
-  /// @brief Sets the parent Thing
+  /// @brief The participant owning this thing
-  /// @param parent The Thing which should become the parnet
+  Participant* owner = nullptr;
-  /// @remark This is equivalent to calling parent->AddChild(this);
+
-  virtual void SetParent(Thing* parent);
+  /// @brief The name of the thing
-  void SetParent(Thing* root, const char* name);
+  const char* name = nullptr;
-  /// @brief Gets the parent Thing
+
+  void SetName(const char* name);
+  const char* GetName() const;
+  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 +145,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 +194,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

@@ -1,4 +1,5 @@
 #include "WindowsParticipant.h"
+#if defined(_WIN32) || defined(_WIN64)
 
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@@ -7,7 +8,6 @@
 #endif
 
 namespace RoboidControl {
-namespace Windows {
 
 void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)
@@ -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 " <<
@@ -102,7 +102,7 @@ void ParticipantUDP::Receive() {
 #endif // _WIN32 || _WIN64
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
 #if defined(_WIN32) || defined(_WIN64)
   char ip_str[INET_ADDRSTRLEN];
   inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@@ -142,5 +142,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   return true;
 }
 
-}  // namespace Windows
 }  // namespace RoboidControl
+
+#endif

Windows/WindowsParticipant.h

@@ -1,15 +1,15 @@
 #pragma once
+#if defined(_WIN32) || defined(_WIN64)
 
 #include "Participants/ParticipantUDP.h"
 
 namespace RoboidControl {
-namespace Windows {
 
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
@@ -18,5 +18,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
   #endif
 };
 
-}  // namespace Windows
 }  // namespace RoboidControl
+#endif

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 milliseconds = Thing::GetTimeMs();
-//     unsigned long startTime = milliseconds;
+  unsigned long startTime = milliseconds;
-//     while (milliseconds < startTime + 1000) {
+  while (milliseconds < startTime + 7000) {
-//       Thing.Update(milliseconds);
+    siteServer->Update();
     
-//       milliseconds = get_time_ms();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//     }
+    milliseconds = Thing::GetTimeMs();
-//     ASSERT_EQ(1, 1);
-//   }
-// }
-
-class ParticipantSuite : public ::testing::Test {
-protected:
-  // SetUp and TearDown can be used to set up and clean up before/after each
-  // test
-  void SetUp() override {
-    // Initialize test data here
   }
+  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();
 
-// TEST_F(ParticipantSuite, ParticipantUDP) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//   ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681);
+    milliseconds = Thing::GetTimeMs();
-
+  }
-//   unsigned long milliseconds = get_time_ms();
+  SUCCEED();
-//   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();
   }