Fixes

Arduino/ArduinoParticipant.cpp

@@ -1,30 +1,32 @@
 #include "ArduinoParticipant.h"
 
-#if defined(ARDUINO)
 #if defined(ARDUINO_ARCH_ESP8266)
+#define HAS_WIFI 1
 #include <ESP8266WiFi.h>
 
 #elif defined(ESP32)
+#define HAS_WIFI 1
 #include <WiFi.h>
 
 #elif defined(UNO_R4)
+#define HAS_WIFI 1
 #include <WiFi.h>
 
 #elif defined(ARDUINO_ARCH_RP2040)  // not functional, for future use
+#define HAS_WIFI 1
 #include <WifiNINA.h>
 
-#endif
 #endif
 
 namespace RoboidControl {
 namespace Arduino {
 
-void ParticipantUDP::Setup(int localPort,
+#if HAS_WIFI
-                             const char* remoteIpAddress,
+WiFiUDP udp;
-                             int remotePort) {
+#endif
+
+void ParticipantUDP::Setup() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  this->remoteIpAddress = remoteIpAddress;
-  this->remotePort = remotePort;
   GetBroadcastAddress();
 
 #if defined(UNO_R4)
@@ -38,9 +40,13 @@ void ParticipantUDP::Setup(int localPort,
     return;
   }
 #endif
-  udp.begin(localPort);
+  std::cout << "starting udp \n";
 
-  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
+  udp.begin(this->port);
+
+  std::cout << "Wifi sync started local " << this->port << ", remote "
+            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
+            << "\n";
 #endif
 }
 
@@ -67,29 +73,19 @@ void ParticipantUDP::Receive() {
     senderAddress.toCharArray(sender_ipAddress, 16);
     unsigned int sender_port = udp.remotePort();
 
-    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
+    // std::cout << "receiving " << packetSize << " bytes, msgId "
-    // sender_port); if (remoteParticipant == nullptr) {
+    //           << (int)this->buffer[0] << "\n";
-    //   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();
   }
-#endif
+#endif  // ARDUINO && HAS_WIFI
 }
 
 bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
-  // << remoteParticipant->port << "\n";
+  //           << remoteParticipant->port << "\n";
 
   int n = 0;
   do {
@@ -102,22 +98,23 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
     udp.write((unsigned char*)buffer, bufferSize);
   } while (udp.endPacket() == 0 && n < 10);
 
-#endif
+#endif  // ARDUINO && HAS_WIFI
   return true;
 }
 
 bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
+  // std::cout << "Publish to " << this->broadcastIpAddress << ":"
+  //           << this->remoteSite->port << "\n";
+
   int bufferSize = msg->Serialize((char*)this->buffer);
   if (bufferSize <= 0)
     return true;
 
-  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
+  udp.beginPacket(this->broadcastIpAddress, this->remoteSite->port);
   udp.write((unsigned char*)buffer, bufferSize);
   udp.endPacket();
 
-  //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
-  //             << this->remotePort << "\n";
 #endif
   return true;
 };

Arduino/ArduinoParticipant.h

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

Arduino/ArduinoUtils.cpp

@@ -45,9 +45,6 @@ struct NssServer {
 bool StartWifi(const char* wifiSsid,
                const char* wifiPassword,
                bool hotspotFallback) {
-#if !defined(HAS_WIFI)
-  return false;
-#else
 #if defined(UNO_R4) || defined(ARDUINO_ARCH_RP2040)
   if (WiFi.status() == WL_NO_MODULE) {
     Serial.println("WiFi not present, WiFiSync is disabled");
@@ -150,13 +147,9 @@ bool StartWifi(const char* wifiSsid,
   }
 
   return (!hotSpotEnabled);
-#endif
 }
 
 void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
-#if !defined(HAS_WIFI)
-  return;
-#else
 #if defined(UNO_R4)  // Uno R4 Wifi does not support this kind of firmware
                      // update (as far as I know)
   return;
@@ -213,6 +206,5 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
     Serial.println(httpCode);
   }
 #endif
-#endif
 }
 #endif

Arduino/Things/DRV8833.h

@@ -16,7 +16,10 @@ class DRV8833Motor : public Thing {
   /// @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);
+  DRV8833Motor(Participant* participant,
+               unsigned char pinIn1,
+               unsigned char pinIn2,
+               bool reverse = false);
   void SetMaxRPM(unsigned int rpm);
 
   virtual void SetAngularVelocity(Spherical velocity) override;
@@ -27,6 +30,12 @@ class DRV8833Motor : public Thing {
   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
 };
 
 class DRV8833 : public Thing {

EspIdf/EspIdfParticipant.cpp

@@ -7,9 +7,9 @@
 namespace RoboidControl {
 namespace EspIdf {
 
-void ParticipantUDP::Setup(int localPort,
+void ParticipantUDP::Setup() {//int localPort,
-                           const char* remoteIpAddress,
+                          //  const char* remoteIpAddress,
-                           int remotePort) {
+                          //  int remotePort) {
 #if defined(IDF_VER)
   std::cout << "Set up UDP\n";
   GetBroadcastAddress();

EspIdf/EspIdfParticipant.h

@@ -11,7 +11,7 @@ namespace EspIdf {
 
 class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
-  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Setup(); //const char* remoteIpAddress, int remotePort);
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);

Participants/ParticipantUDP.cpp

@@ -69,11 +69,11 @@ void ParticipantUDP::SetupUDP(int localPort,
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
       static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+  thisArduino->Setup();  // localPort, remoteIpAddress, remotePort);
 #elif defined(IDF_VER)
   EspIdf::ParticipantUDP* thisEspIdf =
       static_cast<EspIdf::ParticipantUDP*>(this);
-  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
+  thisEspIdf->Setup();  // localPort, remoteIpAddress, remotePort);
 #endif
   this->connected = true;
 }
@@ -105,6 +105,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
       continue;
 
     if (this->isIsolated == false) {
+      //std::cout << "thingg " << thing->name << " pos upd. " << thing->positionUpdated << std::endl;
       PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
       this->Send(thing->owner, poseMsg);
       BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
@@ -158,7 +159,7 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
 #pragma region Send
 
 void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
-                                   Thing* thing) {
+                                   Thing* thing, bool recurse) {
   // std::cout << "Send thing info [" << (int)thing->id << "] \n";
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
   this->Send(remoteParticipant, thingMsg);
@@ -175,6 +176,11 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
   BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
   this->Send(remoteParticipant, customMsg);
   delete customMsg;
+
+  if (recurse) {
+    for (int childIx = 0; childIx < thing->childCount; childIx++)
+      SendThingInfo(remoteParticipant, thing->GetChildByIndex(childIx));
+  }
 }
 
 bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
@@ -337,9 +343,9 @@ void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
 
   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);
+      this->SendThingInfo(sender, thing, false);    
   }
 }
 
@@ -353,7 +359,8 @@ void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
 void ParticipantUDP::Process(Participant* 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";
+            << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
+            << (int)msg->parentId << "\n";
 #endif
 }
 
@@ -394,8 +401,8 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
 
 void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
 #if defined(DEBUG)
-  std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
+  std::cout << this->name << ": process PoseMsg [" << (int)msg->networkId
-            << "/" << (int)msg->networkId << "]\n";
+            << "/" << (int)msg->thingId << "]\n";
 #endif
 }
 

Participants/ParticipantUDP.h

@@ -101,7 +101,7 @@ class ParticipantUDP : public Participant {
 
   virtual void Update(unsigned long currentTimeMs = 0) override;
 
-  void SendThingInfo(Participant* remoteParticipant, Thing* thing);
+  void SendThingInfo(Participant* remoteParticipant, Thing* thing, bool recurse = false);
   void PublishThingInfo(Thing* thing);
 
   bool Send(Participant* remoteParticipant, IMessage* msg);

Thing.cpp

@@ -17,12 +17,6 @@
 
 namespace RoboidControl {
 
-// LocalParticipant* Thing::CheckHiddenParticipant() {
-//   if (isolatedParticipant == nullptr)
-//     isolatedParticipant = new LocalParticipant(0);
-//   return isolatedParticipant;
-// }
-
 Thing::Thing(int thingType)
     : Thing(IsolatedParticipant::Isolated(), thingType) {}
 
@@ -41,27 +35,16 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
 
-  // std::cout << "add thing [" << (int)this->id << "] to owner "
+  this->owner->Add(this, true);
-  //           << this->owner->ipAddress << ":" << this->owner->port << std::endl;
+  std::cout << "add thing [" << (int)this->id << "] to owner "
-  this->owner->Add(this, false);
+            << this->owner->ipAddress << ":" << this->owner->port <<
+            std::endl;
 }
 
-// Thing::Thing(Participant* owner,
+Thing::Thing(Thing* parent, int thingType, unsigned char thingId)
-//             Type thingType,
+    : Thing(parent->owner, thingType, thingId) {
-//              int thingId) {
+    this->parent = parent;
-//   // 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.h

@@ -49,15 +49,15 @@ class Thing {
   /// @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
+  /// <summary>
-  /// @param participant The participant for which this thing is created
+  /// Create a new thing as a child of another thing
-  /// @param networkId The network ID of the thing
+  /// </summary>
-  /// @param thingId The ID of the thing
+  /// <param name="parent">The parent thing</param>
-  /// @param thingType The type of thing
+  /// <param name="thingType">The type of thing</param>
-  // Thing(Participant* participant,
+  /// <param name="thingId">The thing id, level at 0 to automatically generate
-  //       unsigned char networkId,
+  /// an id</param>
-  //       unsigned char thingId,
+  Thing(Thing* parent, int thingType = 0, unsigned char thingId = 0);
-  //       Type thingType = Type::Undetermined);
+
 
   /// @brief The participant managing this thing
   Participant* owner;

Things/AbsoluteEncoder.cpp

@@ -0,0 +1,2 @@
+#include "AbsoluteEncoder.h"
+

Things/AbsoluteEncoder.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ServoMotor.h"
+
+namespace RoboidControl {
+
+class AbsoluteEncoder {
+ public:
+  AbsoluteEncoder() {}
+
+  virtual Angle16 GetActualAngle() = 0;
+  virtual float GetActualVelocity() = 0;
+};
+
+}  // namespace RoboidControl

Things/ControlledMotor.cpp

@@ -0,0 +1,67 @@
+#include "ControlledMotor.h"
+
+namespace RoboidControl {
+
+ControlledMotor::ControlledMotor() { this->type = Thing::ControlledMotor; }
+
+ControlledMotor::ControlledMotor(Motor *motor, IncrementalEncoder *encoder)
+    : ControlledMotor() {
+  this->motor = motor;
+  this->encoder = encoder;
+  // this->rotationDirection = Direction::Forward;
+}
+
+//#include <Arduino.h>
+
+void ControlledMotor::SetTargetSpeed(float speed) {
+  this->currentTargetSpeed = speed;
+  // this->rotationDirection =
+  //     (targetSpeed < 0) ? Direction::Reverse : Direction::Forward;
+  // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise
+  //                                     : Motor::Direction::Clockwise;
+}
+
+void ControlledMotor::Update(unsigned long currentTimeMs) {
+  this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
+  if (this->currentTargetSpeed < 0)
+    this->actualSpeed = -this->actualSpeed;
+
+  float deltaTime = currentTimeMs - this->lastUpdateTime;
+
+  float error = this->currentTargetSpeed - this->actualSpeed;
+  float errorChange = (error - lastError) * deltaTime;
+
+  float delta = error * pidP + errorChange * pidD;
+
+//   Serial.print("  actual Speed ");
+//   Serial.print(actualSpeed);
+//   Serial.print("  target Speed ");
+//   Serial.print(this->currentTargetSpeed);
+//   Serial.print("    motor target speed ");
+//   Serial.print(motor->currentTargetSpeed);
+//   Serial.print(" + ");
+//   Serial.print(error * pidP);
+//   Serial.print(" + ");
+//   Serial.print(errorChange * pidD);
+//   Serial.print(" = ");
+//   Serial.println(motor->currentTargetSpeed + delta);
+
+  motor->SetTargetSpeed(motor->currentTargetSpeed +
+                        delta); // or something like that
+  this->lastUpdateTime = currentTimeMs;
+}
+
+float ControlledMotor::GetActualSpeed() { return actualSpeed; }
+
+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;
+}
+
+}

Things/ControlledMotor.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "IncrementalEncoder.h"
+#include "Motor.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();
+  ControlledMotor(Motor* motor, IncrementalEncoder* encoder);
+
+  inline static bool CheckType(Thing* thing) {
+    return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
+  }
+  float velocity;
+
+  float pidP = 0.1F;
+  float pidD = 0.0F;
+  float pidI = 0.0F;
+
+  void Update(unsigned long currentTimeMs) override;
+
+  /// @brief Set the target speed for the motor controller
+  /// @param speed the target in revolutions per second.
+  virtual void SetTargetSpeed(float speed) override;
+
+  /// @brief Get the actual speed from the encoder
+  /// @return The speed in revolutions per second
+  virtual float GetActualSpeed() override;
+
+  bool Drive(float distance);
+
+  Motor* motor;
+  IncrementalEncoder* encoder;
+
+ protected:
+  float lastUpdateTime = 0;
+  float lastError = 0;
+  // float targetSpeed;
+  float actualSpeed;
+  float netDistance = 0;
+  float startDistance = 0;
+
+  // enum Direction { Forward = 1, Reverse = -1 };
+
+  // Direction rotationDirection;
+
+  bool driving = false;
+  float targetDistance = 0;
+  float lastEncoderPosition = 0;
+};
+
+}  // namespace RoboidControl

Things/IncrementalEncoder.cpp

@@ -0,0 +1,23 @@
+#include "IncrementalEncoder.h"
+
+namespace RoboidControl {
+
+IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution,
+                                       unsigned char distancePerRotation) {
+  this->pulsesPerRevolution = pulsesPerRevolution;
+  this->distancePerRevolution = distancePerRotation;
+}
+
+int IncrementalEncoder::GetPulseCount() { return 0; }
+
+float IncrementalEncoder::GetDistance() { return 0; }
+
+float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
+
+float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
+
+float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; }
+
+}

Things/IncrementalEncoder.h

@@ -0,0 +1,48 @@
+#pragma once
+
+namespace RoboidControl {
+
+/// @brief An Encoder measures the rotations of an axle using a rotary
+/// sensor Some encoders are able to detect direction, while others can not.
+class IncrementalEncoder {
+ 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
+  IncrementalEncoder(unsigned char pulsesPerRevolution = 1,
+                     unsigned char distancePerRevolution = 1);
+
+  /// @brief Get the total number of pulses since the previous call
+  /// @return The number of pulses, is zero or greater
+  virtual int GetPulseCount();
+  /// @brief Get the pulse speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The average pulses per second in the last period.
+  virtual float GetPulsesPerSecond(float currentTimeMs);
+
+  /// @brief Get the distance traveled since the previous call
+  /// @return The distance in meters.
+  virtual float GetDistance();
+
+  /// @brief Get the rotation speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in rotations per second
+  virtual float GetRevolutionsPerSecond(float currentTimeMs);
+
+  /// @brief Get the speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in meters per second.
+  /// @note this value is dependent on the accurate setting of the
+  /// pulsesPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char pulsesPerRevolution = 1;
+  /// @brief The number of revolutions which makes the wheel move forward 1
+  /// meter
+  unsigned char distancePerRevolution = 1;
+};
+
+}  // namespace RoboidControl

Things/Motor.cpp

@@ -0,0 +1,21 @@
+#include "Motor.h"
+
+#include <time.h>
+
+namespace RoboidControl {
+
+Motor::Motor() : Thing(0) { // for now, id should be set properly later
+  this->type = (int)Thing::UncontrolledMotor;
+}
+
+float Motor::GetActualSpeed() { return this->currentTargetSpeed; }
+
+void Motor::SetTargetSpeed(float targetSpeed) {
+  this->currentTargetSpeed = targetSpeed;
+}
+
+float Motor::GetTargetSpeed() { return (this->currentTargetSpeed); }
+
+void Motor::Update(unsigned long currentTimeMs) {}
+
+}

Things/Motor.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "Thing.h"
+
+//#include <time.h>
+
+namespace RoboidControl {
+
+/// @brief A Motor is a Thing which can move parts of the Roboid
+/// @note Currently only rotational motors are supported
+class Motor : public Thing {
+public:
+  /// @brief Default constructor for the Motor
+  Motor();
+
+  /// @brief Motor turning direction
+  enum class Direction { Clockwise = 1, CounterClockwise = -1 };
+  /// @brief The forward turning direction of the motor
+  Direction direction = Direction::Clockwise;
+
+  /// @brief Set the target motor speed
+  /// @param speed The speed between -1 (full backward), 0 (stop) and 1 (full
+  /// forward)
+  virtual void SetTargetSpeed(float speed);
+  /// @brief Get the current target speed of the motor
+  /// @return The speed between -1 (full backward), 0 (stop) and 1 (full
+  /// forward)
+  virtual float GetTargetSpeed();
+
+  /// @brief Get the current actual speed of the motor
+  /// @return The speed between -1 (full backward), 0 (stop) and 1 (full
+  /// forward)
+  virtual float GetActualSpeed();
+
+  virtual void Update(unsigned long currentTimeMs);
+
+  float currentTargetSpeed = 0;
+
+protected:
+};
+
+} // namespace RoboidControl

Things/ServoMotor.cpp

@@ -0,0 +1,113 @@
+#include "ServoMotor.h"
+
+#include "LinearAlgebra/FloatSingle.h"
+#include "Participant.h"
+
+namespace RoboidControl {
+
+ServoMotor::ServoMotor(Participant* participant) : Thing(participant, Thing::Servo, 0) {
+  this->controlMode = ControlMode::Position;
+  this->targetAngle = Angle16();
+  this->hasTargetAngle = false;
+}
+
+ServoMotor::ServoMotor(Thing* parent) : ServoMotor(parent->owner) {
+  std::cout << "new parent " << parent->name << " owner " << parent->owner << std::endl;
+  this->parent = parent;
+}
+
+void ServoMotor::SetTargetAngle(Angle16 angle) {
+  angle = Angle16::Clamp(angle, minAngle, maxAngle);
+
+  if (maxSpeed == 0.0F) {
+    SetAngle(angle);
+    this->limitedTargetAngle = angle;
+  }
+
+  this->controlMode = ControlMode::Position;
+  this->targetAngle = angle;
+  this->hasTargetAngle = true;
+}
+
+Angle16 ServoMotor::GetTargetAngle() {
+  return this->targetAngle;
+}
+
+void ServoMotor::SetMaximumVelocity(float maxVelocity) {
+  this->maxSpeed = maxVelocity;
+}
+
+void ServoMotor::SetTargetVelocity(float targetVelocity) {
+  targetVelocity = Float::Clamp(targetVelocity, -this->maxSpeed, maxSpeed);
+
+  this->controlMode = ControlMode::Velocity;
+  this->targetVelocity = targetVelocity;
+  this->hasTargetAngle = false;  // can't we use the controlMode for this?
+}
+
+float ServoMotor::GetTargetVelocity() {
+  return this->targetVelocity;
+}
+
+void ServoMotor::Update(unsigned long currentTimeMs, bool recurse) {
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing* child = this->GetChild(childIx);
+    if (child != nullptr && child->type == Thing::Servo) {
+      ServoMotor* servo = (ServoMotor*)child;
+      servo->Update(currentTimeMs, recurse);
+    }
+  }
+
+  if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
+    this->lastUpdateTimeMs = currentTimeMs;
+    return;
+  }
+
+  float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;
+
+  // Position control
+  if (controlMode == ControlMode::Position) {
+    if (maxSpeed == 0.0F || hasTargetAngle == false) {
+      this->lastUpdateTimeMs = currentTimeMs;
+      return;
+
+    } else {
+      float angleStep = maxSpeed * deltaTime;
+      float deltaAngle =
+          this->targetAngle.InDegrees() - this->limitedTargetAngle.InDegrees();
+      float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;
+
+      if (absDeltaAngle < angleStep) {
+        this->limitedTargetAngle = this->targetAngle;
+        SetAngle(targetAngle);
+      } else {
+        if (deltaAngle < 0)
+          limitedTargetAngle = limitedTargetAngle - Angle16::Degrees(angleStep);
+        else
+          limitedTargetAngle = limitedTargetAngle + Angle16::Degrees(angleStep);
+      }
+      SetAngle(limitedTargetAngle);
+
+      this->lastUpdateTimeMs = currentTimeMs;
+      return;
+    }
+
+    // Velocity control
+  } else {
+    float newAngle = this->targetAngle.InDegrees() + targetVelocity * deltaTime;
+    newAngle =
+        Float::Clamp(newAngle, minAngle.InDegrees(), maxAngle.InDegrees());
+
+    Angle16 targetAngle = Angle16::Degrees(newAngle);
+    ServoMotor::SetTargetAngle(targetAngle);
+    SetAngle(targetAngle);
+
+    this->lastUpdateTimeMs = currentTimeMs;
+  }
+}
+
+void ServoMotor::SetAngle(Angle16 angle) {
+  this->actualAngle = angle;
+};
+
+}  // namespace RoboidControl

Things/ServoMotor.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include "ControlledMotor.h"
+#include "LinearAlgebra/Angle.h"
+
+namespace RoboidControl {
+
+class ServoMotor : public Thing {
+ public:
+  // Inherit constructors from Thing
+  using Thing::Thing;
+
+  ServoMotor(Participant* participant);
+  ServoMotor(Thing* parent);
+
+  Direction16 rotationAxis = Direction16::up;
+  Angle16 minAngle = Angle16::Degrees(-90);
+  Angle16 maxAngle = Angle16::Degrees(90);
+
+  enum ControlMode { Position, Velocity };
+  ControlMode controlMode = ControlMode::Position;
+
+  Thing* target = nullptr;
+
+  virtual void SetTargetAngle(Angle16 angle);
+  virtual Angle16 GetTargetAngle();
+
+  virtual void SetMaximumVelocity(float maxVelocity);
+
+  virtual void SetTargetVelocity(float velocity);
+  virtual float GetTargetVelocity();
+
+  virtual void Update(unsigned long currentTimeMs, bool recurse) override;
+
+  Angle16 limitedTargetAngle = Angle16();
+
+ protected:
+  bool hasTargetAngle = false;
+  Angle16 targetAngle = Angle16();
+  Angle16 actualAngle = Angle16();
+
+  float maxSpeed = 0.0F;
+
+  float targetVelocity = 0.0F;
+
+  float lastUpdateTimeMs = 0.0F;
+
+  virtual void SetAngle(Angle16 angle);
+};
+
+}  // namespace RoboidControl