AR mode works, but reaction time is slow

Arduino/ArduinoParticipant.cpp

@@ -1,5 +1,9 @@
 #include "ArduinoParticipant.h"
 
+#if !defined(NO_STD)
+#include <iostream>
+#endif
+
 #if defined(ARDUINO)
 #if defined(ARDUINO_ARCH_ESP8266)
 #include <ESP8266WiFi.h>
@@ -29,7 +33,9 @@ void ParticipantUDP::Setup() {
 
 #if defined(UNO_R4)
   if (WiFi.status() == WL_NO_MODULE) {
+#if !defined(NO_STD)
     std::cout << "No network available!\n";
+#endif
     return;
   }
 #else
@@ -42,11 +48,13 @@ void ParticipantUDP::Setup() {
   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
 }
 
 void ParticipantUDP::GetBroadcastAddress() {
@@ -57,8 +65,10 @@ 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() {
@@ -72,19 +82,6 @@ void ParticipantUDP::Receive() {
     senderAddress.toCharArray(sender_ipAddress, 16);
     unsigned int sender_port = udp->remotePort();
 
-    // Participant* remoteParticipant = this->Get(sender_ipAddress,
-    // sender_port); if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->Add(sender_ipAddress,
-    //   sender_port);
-    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
-    //   //           << "\n";
-    //   // std::cout << "New remote participant " <<
-    //   remoteParticipant->ipAddress
-    //   //           << ":" << remoteParticipant->port << " "
-    //   //           << (int)remoteParticipant->networkId << "\n";
-    // }
-
-    // ReceiveData(packetSize, remoteParticipant);
     ReceiveData(packetSize, sender_ipAddress, sender_port);
     packetSize = udp->parsePacket();
   }
@@ -97,15 +94,22 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
   // << remoteParticipant->port << "\n";
 
   int n = 0;
+  int r = 0;
   do {
     if (n > 0) {
+#if !defined(NO_STD)
       std::cout << "Retry sending\n";
+#endif
       delay(10);
     }
     n++;
+
     udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
     udp->write((unsigned char*)buffer, bufferSize);
-  } while (udp->endPacket() == 0 && n < 10);
+    r = udp->endPacket();
+    // On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
+    // It is not cleary yet why
+  } while (r == 0 && n < 10);
 
 #endif
   return true;

Arduino/Things/DRV8833.cpp

@@ -5,6 +5,39 @@
 namespace RoboidControl {
 namespace Arduino {
 
+#pragma region DRV8833
+
+DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
+  this->pinStandby = config.standby;
+  if (pinStandby != 255)
+    pinMode(pinStandby, OUTPUT);
+
+  this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
+  this->motorA->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(this->drv8833.motorA,
+                                       this->drv8833.motorB,
+                                       parent),
+      drv8833(config, this) {}
+
+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
@@ -13,7 +46,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
                            unsigned char pinIn1,
                            unsigned char pinIn2,
                            bool reverse)
-    : Thing(driver->owner) {
+    : Motor() {
   this->SetParent(driver);
 
   this->pinIn1 = pinIn1;
@@ -33,33 +66,20 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
   pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
 #endif
 
-  this->reverse = reverse;
+  // this->reverse = reverse;
 }
 
-void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
-  this->maxRpm = rpm;
-}
+// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
+//   this->maxRpm = rpm;
+// }
 
-void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
-  Thing::SetAngularVelocity(velocity);
-  // ignoring rotation axis for now.
-  // Spherical angularVelocity = this->GetAngularVelocity();
-  float angularSpeed = velocity.distance;  // in degrees/sec
+void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
+  Motor::SetTargetVelocity(motorSpeed);
 
-  float rpm = angularSpeed / 360 * 60;
-  float motorSpeed = rpm / this->maxRpm;
+  uint8_t motorSignal =
+      (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
 
-  uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
-  // 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
+  // std::cout << "moto speed " << this->name << " = " << motorSpeed
   //           << ", motor signal = " << (int)motorSignal << "\n";
 
 #if (ESP32)
@@ -109,43 +129,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
 #endif
 }
 
-DRV8833::DRV8833(Participant* participant,
-                 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(this, pinAIn1, pinAIn2, reverseA);
-  this->motorA->SetName("Motor A");
-  this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB);
-  this->motorB->SetName("Motor B");
-}
-
-DRV8833::DRV8833(Thing* parent,
-                 unsigned char pinAIn1,
-                 unsigned char pinAIn2,
-                 unsigned char pinBIn1,
-                 unsigned char pinBIn2,
-                 unsigned char pinStandby,
-                 bool reverseA,
-                 bool reverseB)
-    : DRV8833(parent->owner,
-              pinAIn1,
-              pinAIn2,
-              pinBIn1,
-              pinBIn2,
-              pinStandby,
-              reverseA,
-              reverseB) {
-                this->SetParent(parent);
-}
+#pragma endregion Motor
 
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DRV8833.h

@@ -1,8 +1,10 @@
 #pragma once
 
 #include <Arduino.h>
+#include "Participants/IsolatedParticipant.h"
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
+#include "Things/Motor.h"
 
 namespace RoboidControl {
 namespace Arduino {
@@ -11,44 +13,46 @@ class DRV8833Motor;
 
 class DRV8833 : public Thing {
  public:
+  struct Configuration {
+    int AIn1;
+    int AIn2;
+    int BIn1;
+    int BIn2;
+    int standby;
+  };
+
   /// @brief Setup a DRV8833 motor controller
-  /// @param pinAIn1 The pin number connected to the AIn1 port
-  /// @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);
-  DRV8833(Thing* parent,
-          unsigned char pinAIn1,
-          unsigned char pinAIn2,
-          unsigned char pinBIn1,
-          unsigned char pinBIn2,
-          unsigned char pinStandby = 255,
-          bool reverseA = false,
-          bool reverseB = false);
+  DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
+
   DRV8833Motor* motorA = nullptr;
   DRV8833Motor* motorB = nullptr;
 
  protected:
   unsigned char pinStandby = 255;
+
+ public:
+  class DifferentialDrive;
 };
 
-/// @brief Support for a DRV8833 motor controller
-class DRV8833Motor : public Thing {
- public:
-  /// @brief Motor turning direction
-  enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
+#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;
+};
+
+#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
@@ -57,16 +61,16 @@ class DRV8833Motor : public Thing {
                unsigned char pinIn1,
                unsigned char pinIn2,
                bool reverse = false);
-  void SetMaxRPM(unsigned int rpm);
+  // void SetMaxRPM(unsigned int rpm);
 
-  virtual void SetAngularVelocity(Spherical velocity) override;
-
-  bool reverse = false;
+  // 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;
+  // unsigned int maxRpm = 200;
 
 #if (ESP32)
   uint8_t in1Ch;
@@ -75,5 +79,7 @@ class DRV8833Motor : public Thing {
 #endif
 };
 
+#pragma endregion Motor
+
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.cpp

@@ -5,19 +5,119 @@
 namespace RoboidControl {
 namespace Arduino {
 
-DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
-    : TouchSensor(participant) {
+#pragma region Digital input
+
+DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) {
   this->pin = pin;
-
-  pinMode(pin, INPUT);
+  pinMode(this->pin, INPUT);
+  std::cout << "digital input start\n";
 }
 
-void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
-  this->touchedSomething = digitalRead(pin) == LOW;
-  // std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
-  // this->touchedSomething << "\n";
-  Thing::Update(currentTimeMs, recursive);
+void DigitalInput::Update(bool recursive) {
+  this->isHigh = digitalRead(this->pin);
+  this->isLow = !this->isHigh;
+  Thing::Update(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->touchedSomething = 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)
+    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
+                    RISING);
+  else if (interruptCount == 1)
+    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;
+  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,
-                      bool recursive = false) override;
+  virtual void Update(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,39 +1,32 @@
 #include "UltrasonicSensor.h"
 
 #include <Arduino.h>
+#include <iostream>
 
 namespace RoboidControl {
 namespace Arduino {
 
-UltrasonicSensor::UltrasonicSensor(Participant* participant,
-                                   unsigned char pinTrigger,
-                                   unsigned char pinEcho)
-    : TouchSensor(participant) {
-  this->pinTrigger = pinTrigger;
-  this->pinEcho = pinEcho;
+UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
+    : Thing(Type::Undetermined, parent) {
+  this->name = "Ultrasonic sensor";
+  this->pinTrigger = config.triggerPin;
+  this->pinEcho = config.echoPin;
 
   pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
   pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
 }
 
-UltrasonicSensor::UltrasonicSensor(Thing* parent,
-                                   unsigned char pinTrigger,
-                                   unsigned char pinEcho)
-    : UltrasonicSensor(parent->owner, pinTrigger, pinEcho) {
-  this->SetParent(parent);
-}
-
 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 =
-      pulseIn(pinEcho, HIGH, 100000);  // the result is in microseconds
+  unsigned long duration_us =
+      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
@@ -44,32 +37,38 @@ 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 > 0 && this->distance <= this->touchDistance);
-
-  // std::cout << "Ultrasonic " << this->touchedSomething << " |  " << (this->distance > 0) << " " <<
-  // (this->distance <= this->touchDistance) << "\n";
-
-  return distance;
+  return this->distance;
 }
 
-void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
-  this->touchedSomething = false;
+void UltrasonicSensor::Update(bool recursive) {
   GetDistance();
-  Thing::Update(currentTimeMs, recursive);
+  Thing::Update(recursive);
 }
 
-// void UltrasonicSensor::ProcessBinary(char* bytes) {
-//   this->touchedSomething = (bytes[0] == 1);
-//   if (this->touchedSomething)
-//     std::cout << "Touching something!\n";
-// }
+#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->touchedSomething |= (this->ultrasonic.distance > 0 &&
+                            this->ultrasonic.distance <= this->touchDistance);
+}
+
+#pragma region Touch sensor
 
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.h

@@ -6,26 +6,19 @@ namespace RoboidControl {
 namespace Arduino {
 
 /// @brief An HC-SR04 ultrasonic distance sensor
-class UltrasonicSensor : public TouchSensor {
+class UltrasonicSensor : Thing {
  public:
-  // Inherit all constructors
-  //using TouchSensor::TouchSensor;
+  struct Configuration {
+    int triggerPin;
+    int echoPin;
+  };
 
-  /// @brief  Setup an ultrasonic sensor
-  /// @param participant The participant to use
-  /// @param pinTrigger The pin number of the trigger signal
-  /// @param pinEcho The pin number of the echo signal
-  UltrasonicSensor(Participant* participant,
-                   unsigned char pinTrigger,
-                   unsigned char pinEcho);
-  UltrasonicSensor(Thing* parent,
-                   unsigned char pinTrigger,
-                   unsigned char pinEcho);
+  UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
 
   // parameters
 
   /// @brief The distance at which the object is considered to be touched
-  float touchDistance = 0.2f;
+  // float touchDistance = 0.2f;
 
   // state
 
@@ -37,15 +30,35 @@ class UltrasonicSensor : public TouchSensor {
   float GetDistance();
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(unsigned long currentTimeMs,
-                      bool recursive = false) override;
+  virtual void Update(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 TouchSensor;
 };
 
+#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 region Touch sensor
+
 }  // namespace Arduino
 }  // namespace RoboidControl

CMakeLists.txt

@@ -19,13 +19,13 @@ if(ESP_PLATFORM)
         REQUIRES esp_netif esp_wifi
     )
 else() 
+    set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
+    set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
     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(

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 DirectionOf<signed short>;
+template class LinearAlgebra::DirectionOf<float>;
+template class LinearAlgebra::DirectionOf<signed short>;
+}

LinearAlgebra/Direction.h

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

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 PolarOf<signed short>;
+template class LinearAlgebra::PolarOf<float>;
+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) {

Messages/NetworkIdMsg.cpp

@@ -1,5 +1,7 @@
 #include "NetworkIdMsg.h"
 
+#include <iostream>
+
 namespace RoboidControl {
 
 NetworkIdMsg::NetworkIdMsg(const char* buffer) {

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/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 && thing->GetParent() != nullptr)) {
+  if (thing->positionUpdated || (force && thing->IsRoot())) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
   }
-  if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) {
+  if (thing->orientationUpdated || (force && thing->IsRoot())) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
   }

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/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 (parent != nullptr)
-    this->parentId = parent->id;
-  else
+  if (thing->IsRoot())
     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
-            << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl;
+  std::cout << "Send ThingMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->thingType << " "
+            << (int)this->parentId << std::endl;
 #endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;

Participant.cpp

@@ -8,7 +8,26 @@ namespace RoboidControl {
 
 ParticipantRegistry Participant::registry;
 
+Participant* Participant::LocalParticipant = new Participant();
+
+void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
+  LocalParticipant = &newParticipant;
+  std::cout << "Replaced local participant" << std::endl;
+}
+
+Participant::Participant() {
+  std::cout << "P\n";
+  //this->root.name = "Isolated";
+  this->root = new Thing(this);
+  this->root->name = "Root";
+  this->Add(this->root);
+}
+
 Participant::Participant(const char* ipAddress, int port) {
+  // Add the root thing to the list of things, because we could not do that
+  // earlier (I think)
+  this->Add(this->root);
+
   // make a copy of the ip address string
   int addressLength = (int)strlen(ipAddress);
   int stringLength = addressLength + 1;
@@ -26,14 +45,14 @@ Participant::Participant(const char* ipAddress, int port) {
 }
 
 Participant::~Participant() {
-  registry.Remove(this);
+  // registry.Remove(this);
   delete[] this->ipAddress;
 }
 
-void Participant::Update(unsigned long currentTimeMs) {
+void Participant::Update() {
   for (Thing* thing : this->things) {
     if (thing != nullptr)
-      thing->Update(currentTimeMs, true);
+      thing->Update(true);
   }
 }
 
@@ -112,23 +131,26 @@ void Participant::Remove(Thing* thing) {
 #pragma region ParticipantRegistry
 
 Participant* ParticipantRegistry::Get(const char* ipAddress,
-                                                 unsigned int port) {
+                                      unsigned int port) {
+#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;
+      // 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;
@@ -136,11 +158,12 @@ Participant* ParticipantRegistry::Get(unsigned char 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) {
+                                      unsigned int port) {
   Participant* participant = new Participant(ipAddress, port);
   Add(participant);
   return participant;
@@ -152,19 +175,22 @@ void ParticipantRegistry::Add(Participant* participant) {
 
   if (foundParticipant == nullptr) {
 #if defined(NO_STD)
-    this->things[this->thingCount++] = thing;
+    // this->things[this->thingCount++] = thing;
 #else
     ParticipantRegistry::participants.push_back(participant);
 #endif
-    std::cout << "Add participant " << participant->ipAddress << ":"
-              << 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";
+    //   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";
   }
 }
 
@@ -172,9 +198,15 @@ 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
 

Participant.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include "Thing.h"
 
 namespace RoboidControl {
@@ -31,13 +32,21 @@ class ParticipantRegistry {
   /// @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:
-#if defined(NO_STD)
-#else
   /// @brief The list of known participants
   std::list<Participant*> participants;
 #endif
@@ -51,6 +60,9 @@ class ParticipantRegistry {
 /// reference to remote participants.
 class Participant {
  public:
+  /// @brief The name of the participant
+  const char* name = "Participant";
+
   /// @brief The Ip Address of a participant.
   const char* ipAddress = "0.0.0.0";
   /// @brief The port number for UDP communication with the participant.
@@ -59,6 +71,7 @@ class Participant {
   /// @brief The network Id to identify the participant
   unsigned char networkId = 0;
 
+  Participant();
   /// @brief Create a new participant with the given communcation info
   /// @param ipAddress The IP address of the participant
   /// @param port The UDP port of the participant
@@ -66,6 +79,11 @@ class Participant {
   /// @brief Destructor for the participant
   ~Participant();
 
+  static Participant* LocalParticipant;
+  static void ReplaceLocalParticipant(Participant& newParticipant);
+
+  Thing* root = new Thing(this);
+
  public:
 #if defined(NO_STD)
   unsigned char thingCount = 0;
@@ -88,7 +106,7 @@ class Participant {
 
   /// @brief Update all things for this participant
   /// @param currentTimeMs The current time in milliseconds (optional)
-  virtual void Update(unsigned long currentTimeMs = 0);
+  virtual void Update();
 
  public:
   static ParticipantRegistry registry;

Participants/ParticipantUDP.cpp

@@ -1,30 +1,49 @@
 #include "ParticipantUDP.h"
 
+#include "Participant.h"
 #include "Thing.h"
 
 #include "Arduino/ArduinoParticipant.h"
 #include "EspIdf/EspIdfParticipant.h"
-#include "Windows/WindowsParticipant.h"
 #include "Posix/PosixParticipant.h"
+#include "Windows/WindowsParticipant.h"
 
 #include <string.h>
 
 namespace RoboidControl {
 
+#pragma region Init
+
 ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
+  this->name = "ParticipantUDP";
   this->remoteSite = nullptr;
   if (this->port == 0)
     this->isIsolated = true;
   Participant::registry.Add(this);
+
+  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
 }
 
 ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
     : Participant("127.0.0.1", localPort) {
+  this->name = "ParticipantUDP";
   if (this->port == 0)
     this->isIsolated = true;
   else
     this->remoteSite = new Participant(ipAddress, port);
   Participant::registry.Add(this);
+
+  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
 }
 
 static ParticipantUDP* isolatedParticipant = nullptr;
@@ -64,11 +83,18 @@ void ParticipantUDP::SetupUDP(int localPort,
   this->connected = true;
 }
 
+#pragma endregion Init
+
 #pragma region Update
 
-void ParticipantUDP::Update(unsigned long currentTimeMs) {
-  if (currentTimeMs == 0)
-    currentTimeMs = Thing::GetTimeMs();
+// The update order
+// 1. receive external messages
+// 2. update the state
+// 3. send out the updated messages
+void ParticipantUDP::Update() {
+  unsigned long currentTimeMs = Thing::GetTimeMs();
+
+  PrepMyThings();
 
   if (this->isIsolated == false) {
     if (this->connected == false)
@@ -88,15 +114,26 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
     this->ReceiveUDP();
   }
 
-  UpdateMyThings(currentTimeMs);
-  UpdateOtherThings(currentTimeMs);
+  UpdateMyThings();
+  UpdateOtherThings();
 }
 
-void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
+void ParticipantUDP::PrepMyThings() {
+  for (Thing* thing : this->things) {
+    if (thing == nullptr)
+      continue;
+      
+    thing->PrepareForUpdate();
+  }
+}
+
+void ParticipantUDP::UpdateMyThings() {
+  // std::cout << this->things.size() << std::endl;
   for (Thing* thing : this->things) {
     if (thing == nullptr)  // || thing->GetParent() != nullptr)
       continue;
 
+    // std::cout << thing->name << "\n";
     if (thing->hierarchyChanged) {
       if (!(this->isIsolated || this->networkId == 0)) {
         ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
@@ -111,12 +148,14 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
       }
     }
 
+    // std::cout << "B\n";
     // Why don't we do recursive?
     // Because when a thing creates a thing in the update,
     // that new thing is not sent out (because of hierarchyChanged)
     // before it is updated itself: it is immediatedly updated!
-    thing->Update(currentTimeMs, false);
+    thing->Update(false);
 
+    // std::cout << "C\n";
     if (!(this->isIsolated || this->networkId == 0)) {
       if (thing->terminate) {
         DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
@@ -137,20 +176,28 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
         delete binaryMsg;
       }
     }
+    // std::cout << "D\n";
     if (thing->terminate)
       this->Remove(thing);
+    // std::cout << "E\n";
   }
 }
 
-void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) {
+void ParticipantUDP::UpdateOtherThings() {
+#if defined(NO_STD)
+  Participant** participants = Participant::registry.GetAll();
+  for (int ix = 0; ix < Participant::registry.count; ix++) {
+    Participant* participant = participants[ix];
+#else
   for (Participant* participant : Participant::registry.GetAll()) {
+#endif
     if (participant == nullptr || participant == this)
       continue;
 
     // 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(currentTimeMs);
+    participant->Participant::Update();
     if (this->isIsolated)
       continue;
 
@@ -195,8 +242,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
   if (bufferSize <= 0)
     return true;
 
-  // std::cout << "send msg " << (int)this->buffer[0] << " to "
-  // << remoteParticipant->ipAddress << std::endl;
+  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
+  //           << " to " << remoteParticipant->ipAddress << std::endl;
 
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
@@ -373,14 +420,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
 
 void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
 #if defined(DEBUG)
-  std::cout << this->name << ": process SiteMsg " << (int)this->networkId
+  std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
             << " -> " << (int)msg->networkId << "\n";
 #endif
 
   if (this->networkId != msg->networkId) {
     this->networkId = msg->networkId;
 
-    // std::cout << this->things.size() << " things\n";
+    std::cout << this->things.size() << " things\n";
     for (Thing* thing : this->things)
       this->SendThingInfo(sender, thing);
   }
@@ -441,7 +488,7 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
 }
 
 void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
-#if !defined(DEBUG)
+#if !defined(DEBUG) && !defined(NO_STD)
   std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
             << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
 #endif

Participants/ParticipantUDP.h

@@ -78,9 +78,6 @@ class ParticipantUDP : public Participant {
   /// local network
   long publishInterval = 3000;  // 3 seconds
 
-  /// @brief The name of the participant
-  const char* name = "ParticipantUDP";
-
  protected:
   char buffer[1024];
 
@@ -100,13 +97,15 @@ class ParticipantUDP : public Participant {
 #pragma region Update
 
  public:
-  virtual void Update(unsigned long currentTimeMs = 0) override;
+  virtual void Update() override;
 
  protected:
   unsigned long nextPublishMe = 0;
 
-  virtual void UpdateMyThings(unsigned long currentTimeMs);
-  virtual void UpdateOtherThings(unsigned long currentTimeMs);
+  /// @brief Prepare the local things for the next update
+  virtual void PrepMyThings();
+  virtual void UpdateMyThings();
+  virtual void UpdateOtherThings();
 
 #pragma endregion Update
 

Participants/SiteServer.cpp

@@ -22,16 +22,22 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) {
 
 #pragma region Update
 
-void SiteServer::UpdateMyThings(unsigned long currentTimeMs) {
+void SiteServer::UpdateMyThings() {
   for (Thing* thing : this->things) {
     if (thing == nullptr)
       continue;
 
-    thing->Update(currentTimeMs, true);
+    thing->Update(true);
 
     if (this->isIsolated == false) {
       // Send to all other participants
+#if defined(NO_STD)
+      Participant** participants = Participant::registry.GetAll();
+      for (int ix = 0; ix < Participant::registry.count; ix++) {
+        Participant* participant = participants[ix];
+#else
       for (Participant* participant : Participant::registry.GetAll()) {
+#endif
         if (participant == nullptr || participant == this)
           continue;
 
@@ -66,15 +72,24 @@ void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->thingId);
   if (thing == nullptr)
-    new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
+    // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
+    // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
+    //thing = new Thing(msg->thingType, sender->root);
+    ;
+  thing->id = msg->thingId;
 
   if (msg->parentId != 0) {
     thing->SetParent(Get(msg->parentId));
-    if (thing->GetParent() != nullptr)
+    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(nullptr);
+    thing->SetParent(Thing::LocalRoot());
 }
 
 #pragma endregion Receive

Participants/SiteServer.h

@@ -24,7 +24,7 @@ class SiteServer : public ParticipantUDP {
 
 #pragma region Update
 
-  virtual void UpdateMyThings(unsigned long currentTimeMs) override;
+  virtual void UpdateMyThings() override;
 
 #pragma endregion Update
 

Posix/PosixParticipant.cpp

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

Posix/PosixParticipant.h

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

Thing.cpp

@@ -5,6 +5,7 @@
 #include "Participants/IsolatedParticipant.h"
 
 #include <string.h>
+// #include <iostream>
 
 #if defined(ARDUINO)
 #include "Arduino.h"
@@ -19,14 +20,31 @@ namespace RoboidControl {
 
 #pragma region Init
 
-Thing::Thing(unsigned char thingType)
-    : Thing(IsolatedParticipant::Isolated(), thingType) {}
+Thing* Thing::LocalRoot() {
+  Participant* p = Participant::LocalParticipant;
+  Thing* localRoot = p->root;
+  return localRoot;
+}
+
+// Only use this for root things
+Thing::Thing(Participant* owner) {
+  this->type = Type::Roboid;  // should become root
+
+  this->position = Spherical::zero;
+  this->positionUpdated = true;
+  this->orientation = SwingTwist::identity;
+  this->orientationUpdated = true;
+  this->hierarchyChanged = true;
+
+  this->linearVelocity = Spherical::zero;
+  this->angularVelocity = Spherical::zero;
 
-Thing::Thing(Participant* owner,
-             unsigned char thingType,
-             unsigned char thingId) {
   this->owner = owner;
-  this->id = thingId;
+  //this->owner->Add(this, true);
+  std::cout << this->owner->name << ": New root thing " << std::endl;
+}
+
+Thing::Thing(unsigned char thingType, Thing* parent) {
   this->type = thingType;
 
   this->position = Spherical::zero;
@@ -38,17 +56,25 @@ Thing::Thing(Participant* owner,
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
 
-  // std::cout << "add thing [" << (int)this->id << "] to owner "
-  //           << this->owner->ipAddress << ":" << this->owner->port <<
-  //           std::endl;
+  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(Thing* parent, unsigned char thingType, unsigned char thingId)
-    : Thing(parent->owner, thingType, thingId) {
-  this->SetParent(parent);
+Thing::~Thing() {
+  std::cout << "Destroy thing " << this->name << std::endl;
 }
 
+// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
+// unsigned char thingId) {
+//   Thing thing = Thing(owner, thingType);
+//   thing.id = thingId;
+//   return thing;
+// }
+
 #pragma endregion Init
 
 void Thing::SetName(const char* name) {
@@ -77,6 +103,19 @@ void Thing::SetParent(Thing* parent) {
   this->hierarchyChanged = true;
 }
 
+// void Thing::SetParent(Thing* parent) {
+//   parent->AddChild(this);
+//   this->hierarchyChanged = true;
+// }
+
+// const Thing& Thing::GetParent() {
+//   return *this->parent;
+// }
+
+bool Thing::IsRoot() const {
+  return this == LocalRoot() || this->parent == nullptr;  //&Thing::Root;
+}
+
 // void Thing::SetParent(Thing* root, const char* name) {
 //   Thing* thing = root->FindChild(name);
 //   if (thing != nullptr)
@@ -130,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) {
     }
   }
 
-  child->parent = nullptr;
+  child->parent = Thing::LocalRoot();
 
   delete[] this->children;
   this->children = newChildren;
@@ -221,7 +260,8 @@ Spherical Thing::GetAngularVelocity() {
 
 unsigned long Thing::GetTimeMs() {
 #if defined(ARDUINO)
-  return millis();
+  unsigned long ms = millis();
+  return ms;
 #else
   auto now = std::chrono::steady_clock::now();
   auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
@@ -230,11 +270,13 @@ unsigned long Thing::GetTimeMs() {
 #endif
 }
 
-void Thing::Update(bool recursive) {
-  Update(GetTimeMs(), recursive);
-}
+// void Thing::Update(bool recursive) {
+//   Update(GetTimeMs(), recursive);
+// }
 
-void Thing::Update(unsigned long currentTimeMs, bool recursive) {
+void Thing::PrepareForUpdate() {}
+
+void Thing::Update(bool recursive) {
   // if (this->positionUpdated || this->orientationUpdated)
   //   OnPoseChanged callback
   this->positionUpdated = false;
@@ -245,17 +287,18 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) {
   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(currentTimeMs, recursive);
+      child->Update(recursive);
     }
   }
 }
 
-void Thing::UpdateThings(unsigned long currentTimeMs) {
-  IsolatedParticipant::Isolated()->Update(currentTimeMs);
+void Thing::UpdateThings() {
+  IsolatedParticipant::Isolated()->Update();
 }
 
 #pragma endregion Update

Thing.h

@@ -32,6 +32,7 @@ class Thing {
     ControlledMotor,
     UncontrolledMotor,
     Servo,
+    IncrementalEncoder,
     // Other
     Roboid,
     Humanoid,
@@ -40,19 +41,23 @@ class Thing {
   };
 
 #pragma region Init
+  static Thing* LocalRoot();
 
-  /// @brief Create a new thing without communication abilities
-  /// @param thingType The type of thing (can use Thing::Type)
-  Thing(unsigned char thingType = Type::Undetermined);
+ private:
+  // Special constructor to create a root thing
+  Thing(Participant* parent);
+  // Which can only be used by the Participant
+  friend class Participant;
 
-  /// @brief Create a new thing for a participant
-  /// @param owner The owning participant
+ public:
+  /// @brief Create a new thing
   /// @param thingType The type of thing (can use Thing::Type)
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  Thing(Participant* owner,
-        unsigned char thingType = Type::Undetermined,
-        unsigned char thingId = 0);
+  /// @param parent (optional) The parent thing
+  /// 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 a root thing.
+  Thing(unsigned char thingType = Thing::Type::Undetermined,
+        Thing* parent = LocalRoot());
 
   /// @brief Create a new child thing
   /// @param parent The parent thing
@@ -60,7 +65,12 @@ class Thing {
   /// @param thingId The ID of the thing, leave out or set to zero to generate
   /// an ID
   /// @note The owner will be the same as the owner of the parent thing
-  Thing(Thing* parent, unsigned char thingType = 0, unsigned char thingId = 0);
+
+  ~Thing();
+
+  static Thing Reconstruct(Participant* owner,
+                           unsigned char thingType,
+                           unsigned char thingId);
 
 #pragma endregion Init
 
@@ -72,6 +82,7 @@ class Thing {
 
   /// @brief The participant managing this thing
   Participant* owner = nullptr;
+
   /// @brief The ID of the thing
   unsigned char id = 0;
 
@@ -106,11 +117,15 @@ class Thing {
 
   /// @brief Sets the parent of this Thing
   /// @param parent The Thing which should become the parent
-  virtual void SetParent(Thing* parent);
+  // virtual void SetParent(Thing* parent);
+  void SetParent(Thing* parent);
   /// @brief Gets the parent of this Thing
   /// @return The parent Thing
+  // Thing* GetParent();
   Thing* GetParent();
 
+  bool IsRoot() const;
+
   /// @brief The number of children
   unsigned char childCount = 0;
   /// @brief Get a child by index
@@ -198,9 +213,11 @@ class Thing {
   /// parent's orientation
   SwingTwist orientation;
 
-  /// @brief The linear velocity of the thing in local space, in meters per second
+  /// @brief The linear velocity of the thing in local space, in meters per
+  /// second
   Spherical linearVelocity;
-  /// @brief The angular velocity of the thing in local space, in degrees per second
+  /// @brief The angular velocity of the thing in local space, in degrees per
+  /// second
   Spherical angularVelocity;
 
 #pragma endregion Pose
@@ -208,20 +225,19 @@ class Thing {
 #pragma region Update
 
  public:
-  /// @brief Get the current time in milliseconds
-  /// @return The current time in milliseconds
-  static unsigned long GetTimeMs();
-
-  /// @brief Updates the state of the thing
-  void Update(bool recursive = false);
+  virtual void PrepareForUpdate();
 
   /// @brief Updates the state of the thing
   /// @param currentTimeMs The current clock time in milliseconds; if this is
   /// zero, the current time is retrieved automatically
   /// @param recurse When true, this will Update the descendants recursively
-  virtual void Update(unsigned long currentTimeMs, bool recurse = false);
+  virtual void Update(bool recurse = false);
 
-  static void UpdateThings(unsigned long currentTimeMs);
+  static void UpdateThings();
+
+  /// @brief Get the current time in milliseconds
+  /// @return The current time in milliseconds
+  static unsigned long GetTimeMs();
 
 #pragma endregion Update
 

Things/ControlledMotor.cpp

@@ -0,0 +1,54 @@
+#include "ControlledMotor.h"
+
+namespace RoboidControl {
+
+ControlledMotor::ControlledMotor(Motor* motor,
+                                 RelativeEncoder* encoder,
+                                 Thing* parent)
+    : Motor(parent), motor(motor), encoder(encoder) {
+  this->type = Type::ControlledMotor;
+  // encoder.SetParent(*this);
+  //  Thing parent = motor.GetParent();
+  //  this->SetParent(parent);
+}
+
+void ControlledMotor::SetTargetVelocity(float velocity) {
+  this->targetVelocity = velocity;
+  this->rotationDirection =
+      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
+}
+
+void ControlledMotor::Update(bool recurse) {
+  encoder->Update(false);
+
+  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
+
+  unsigned long currentTimeMs = GetTimeMs();
+  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
+  this->lastUpdateTime = currentTimeMs;
+
+  float error = this->targetVelocity - this->actualVelocity;
+  float acceleration =
+      error * timeStep * pidP;  // Just P is used at this moment
+  std::cout << "motor acc. " << acceleration << std::endl;
+  motor->SetTargetVelocity(targetVelocity +
+                          acceleration);  // 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,38 @@
+#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 = 1;
+  float pidD = 0;
+  float pidI = 0;
+
+  /// @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 lastUpdateTime;
+};
+
+}  // namespace RoboidControl

Things/DifferentialDrive.cpp

@@ -1,13 +1,28 @@
 #include "DifferentialDrive.h"
 
+#include "Messages/LowLevelMessages.h"
+
 namespace RoboidControl {
 
-  DifferentialDrive::DifferentialDrive() : Thing(Thing::Type::DifferentialDrive) {}
+DifferentialDrive::DifferentialDrive(Thing* parent)
+    : Thing(Type::DifferentialDrive, parent) {
+  this->name = "Differential drive";
 
-DifferentialDrive::DifferentialDrive(Participant* participant, unsigned char thingId)
-    : Thing(participant, Thing::Type::DifferentialDrive, thingId) {}
+  this->leftWheel = new Motor(this);
+  this->leftWheel->name = "Left motor";
 
-DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) : Thing(parent, Thing::Type::DifferentialDrive, thingId) {}
+  this->rightWheel = new Motor(this);
+  this->rightWheel->name = "Right motor";
+}
+
+DifferentialDrive::DifferentialDrive(Motor* leftMotor,
+                                     Motor* rightMotor,
+                                     Thing* parent)
+    : Thing(Type::DifferentialDrive, parent) {
+  this->name = "Differential drive";
+  this->leftWheel = leftMotor;
+  this->rightWheel = rightMotor;
+}
 
 void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
                                            float wheelSeparation) {
@@ -24,27 +39,38 @@ 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;
-  ;
-  if (leftWheel != nullptr)
-    this->leftWheel->SetPosition(Spherical(distance, Direction::left));
+  this->leftWheel = &leftMotor;
+  this->leftWheel->SetPosition(Spherical(distance, Direction::left));
 
-  this->rightWheel = rightWheel;
-  if (rightWheel != nullptr)
-    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
+  this->rightWheel = &rightMotor;
+  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(
-        Spherical(speedRight, Direction::right));
+    this->rightWheel->SetTargetVelocity(velocityRight);
 }
 
-void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
+void DifferentialDrive::Update(bool recursive) {
   if (this->linearVelocityUpdated) {
     // this assumes forward velocity only....
     float linearVelocity = this->GetLinearVelocity().distance;
@@ -65,7 +91,14 @@ void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
 
     this->SetWheelVelocity(speedLeft, speedRight);
   }
-  Thing::Update(currentMs, recursive);
+  Thing::Update(recursive);
+}
+
+int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = this->leftWheel->id;
+  data[(*ix)++] = this->rightWheel->id;
+  LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
+  return 4;
 }
 
 }  // namespace RoboidControl

Things/DifferentialDrive.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Thing.h"
+#include "Motor.h"
 
 namespace RoboidControl {
 
@@ -9,18 +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
-  DifferentialDrive();
-  /// @brief Create a differential drive with networking support
-  /// @param participant The local participant
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  DifferentialDrive(Participant* participant, unsigned char thingId = 0);
   /// @brief Create a new child differential drive
   /// @param parent The parent thing
   /// @param thingId The ID of the thing, leave out or set to zero to generate
   /// an ID
-  DifferentialDrive(Thing* parent, unsigned char thingId = 0);
+  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
@@ -30,10 +26,13 @@ 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.
@@ -43,21 +42,25 @@ class DifferentialDrive : public Thing {
   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,13 +2,18 @@
 
 namespace RoboidControl {
 
-DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
+//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
 
-DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
-    : Thing(owner, Type::Switch, thingId) {}
+// DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
+//     : Thing(owner, Type::Switch, thingId) {}
 
-DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId)
-    : Thing(parent, Type::Switch) {}
+// DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId)
+//     : Thing(parent, Type::Switch) {}
+
+// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {}
+
+// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {}
+DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {}
 
 int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
   bytes[(*ix)++] = state ? 1 : 0;

Things/DigitalSensor.h

@@ -8,17 +8,18 @@ namespace RoboidControl {
 class DigitalSensor : public Thing {
  public:
   /// @brief Create a digital sensor without communication abilities
-  DigitalSensor();
+  //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, unsigned char thingId = 0);
+  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); //, unsigned char thingId = 0);
+  DigitalSensor(Thing* parent = Thing::LocalRoot());
 
   /// @brief The sigital state
   bool state = 0;

Things/Motor.cpp

@@ -0,0 +1,16 @@
+#include "Motor.h"
+
+namespace RoboidControl {
+
+Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
+
+void Motor::SetTargetVelocity(float targetSpeed) {
+    this->targetVelocity = targetSpeed;
+}
+
+int Motor::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = this->targetVelocity * 127.0f;
+  return 1;
+}
+
+}  // namespace RoboidControl

Things/Motor.h

@@ -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
+
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  // virtual void ProcessBinary(char* bytes) override;
+
+ //protected:
+  float targetVelocity = 0;
+};
+
+}  // namespace RoboidControl

Things/RelativeEncoder.cpp

@@ -0,0 +1,21 @@
+#include "RelativeEncoder.h"
+
+namespace RoboidControl {
+
+RelativeEncoder::RelativeEncoder(Thing* parent)
+    : Thing(Type::IncrementalEncoder, parent) {}
+
+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;
+
+  /// @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,15 @@
 
 namespace RoboidControl {
 
-TemperatureSensor::TemperatureSensor(Participant* participant,
-                                     unsigned char thingId)
-    : Thing(participant, Type::TemperatureSensor, thingId) {}
+// TemperatureSensor::TemperatureSensor(Participant* participant,
+//                                      unsigned char thingId)
+//     : Thing(participant, Type::TemperatureSensor, thingId) {}
+
+// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
+
+TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
+
+// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}
 
 void TemperatureSensor::SetTemperature(float temp) {
   this->temperature = temp;

Things/TemperatureSensor.h

@@ -12,7 +12,9 @@ class TemperatureSensor : public Thing {
   /// @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;

Things/TouchSensor.cpp

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

Things/TouchSensor.h

@@ -8,23 +8,19 @@ namespace RoboidControl {
 class TouchSensor : public Thing {
 // Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
  public:
-  /// @brief Create a touch sensor without communication abilities
-  TouchSensor();
-  /// @brief Create a touch 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
-  TouchSensor(Participant* owner, unsigned char thingId = 0);
   /// @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
-  TouchSensor(Thing* parent, unsigned char thingId = 0);
+  TouchSensor(Thing* parent = Thing::LocalRoot());
 
   /// @brief Value which is true when the sensor is touching something, false
   /// otherwise
   bool touchedSomething = false;
 
+  virtual void PrepareForUpdate() override;
+  virtual void Update(bool recursive) override;
+
   /// @brief Function used to generate binary data for this touch sensor
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
@@ -32,6 +28,8 @@ class TouchSensor : public Thing {
   /// @brief Function used to process binary data received for this touch sensor
   /// @param bytes The binary data to process
   virtual void ProcessBinary(char* bytes) override;
+protected:
+  bool externalTouch = false;
 };
 
 }  // namespace RoboidControl

examples/BB2B.cpp

@@ -17,26 +17,26 @@ 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.touchedSomething) ? -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.touchedSomething) ? -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)

examples/README.md

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

test/participant_test.cc

@@ -7,6 +7,9 @@
 #include "Participants/SiteServer.h"
 #include "Thing.h"
 
+#include <chrono>
+#include <thread>
+
 using namespace RoboidControl;
 
 TEST(Participant, Participant) {
@@ -15,7 +18,7 @@ TEST(Participant, Participant) {
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 7000) {
-    participant->Update(milliseconds);
+    participant->Update();
     
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     milliseconds = Thing::GetTimeMs();
@@ -29,7 +32,7 @@ TEST(Participant, SiteServer) {
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 7000) {
-    siteServer->Update(milliseconds);
+    siteServer->Update();
     
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     milliseconds = Thing::GetTimeMs();
@@ -44,8 +47,8 @@ TEST(Participant, SiteParticipant) {
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 7000) {
-    siteServer->Update(milliseconds);
-    participant->Update(milliseconds);
+    siteServer->Update();
+    participant->Update();
 
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     milliseconds = Thing::GetTimeMs();
@@ -63,8 +66,8 @@ TEST(Participant, ThingMsg) {
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 7000) {
-    siteServer->Update(milliseconds);
-    participant->Update(milliseconds);
+    siteServer->Update();
+    participant->Update();
 
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     milliseconds = Thing::GetTimeMs();

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::UpdateThings();
 
     milliseconds = Thing::GetTimeMs();
   }
@@ -29,7 +29,7 @@ TEST(RoboidControlSuite, IsolatedParticipant) {
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 1000) {
-    participant->Update(milliseconds);
+    participant->Update();
 
     milliseconds = Thing::GetTimeMs();
   }