Fixes

Arduino/ArduinoParticipant.cpp

@@ -1,25 +1,29 @@
 #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 {
 
-WiFiUDP* udp;
+#if HAS_WIFI
+WiFiUDP udp;
+#endif
 
 void ParticipantUDP::Setup() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
@@ -36,14 +40,13 @@ void ParticipantUDP::Setup() {
     return;
   }
 #endif
+  std::cout << "starting udp \n";
 
-  udp = new WiFiUDP();
-  udp->begin(this->port);
+  udp.begin(this->port);
 
-  std::cout << "Wifi sync started local " << this->port;
-  if (this->remoteSite != nullptr)
-    std::cout << ", remote " << this->remoteSite->ipAddress << ":"
-              << this->remoteSite->port << "\n";
+  std::cout << "Wifi sync started local " << this->port << ", remote "
+            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
+            << "\n";
 #endif
 }
 
@@ -61,38 +64,28 @@ void ParticipantUDP::GetBroadcastAddress() {
 
 void ParticipantUDP::Receive() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  int packetSize = udp->parsePacket();
+  int packetSize = udp.parsePacket();
   while (packetSize > 0) {
-    udp->read(buffer, packetSize);
+    udp.read(buffer, packetSize);
 
-    String senderAddress = udp->remoteIP().toString();
+    String senderAddress = udp.remoteIP().toString();
     char sender_ipAddress[16];
     senderAddress.toCharArray(sender_ipAddress, 16);
-    unsigned int sender_port = udp->remotePort();
+    unsigned int sender_port = udp.remotePort();
 
-    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
-    // sender_port); if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(sender_ipAddress,
-    //   sender_port);
-    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
-    //   //           << "\n";
-    //   // std::cout << "New remote participant " <<
-    //   remoteParticipant->ipAddress
-    //   //           << ":" << remoteParticipant->port << " "
-    //   //           << (int)remoteParticipant->networkId << "\n";
-    // }
-
-    // ReceiveData(packetSize, remoteParticipant);
+    // std::cout << "receiving " << packetSize << " bytes, msgId "
+    //           << (int)this->buffer[0] << "\n";
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    packetSize = udp->parsePacket();
+
+    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 {
@@ -101,26 +94,27 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
       delay(10);
     }
     n++;
-    udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
-    udp->write((unsigned char*)buffer, bufferSize);
-  } while (udp->endPacket() == 0 && n < 10);
+    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
+    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->port);
-  udp->write((unsigned char*)buffer, bufferSize);
-  udp->endPacket();
+  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,19 +2,31 @@
 
 #include "Participants/ParticipantUDP.h"
 
+// #if defined(ARDUINO_ARCH_ESP8266) || defined(ESP32) || defined(UNO_R4) || \
+//     defined(ARDUINO_ARCH_RP2040)
+// #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();
+  void Setup();  // const char* remoteIpAddress, int remotePort);
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
+//#if defined(HAS_WIFI)
   char* broadcastIpAddress = nullptr;
 
+//#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.cpp

@@ -5,27 +5,18 @@
 namespace RoboidControl {
 namespace Arduino {
 
-#if (ESP32)
-uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
-#endif
-
-DRV8833Motor::DRV8833Motor(Participant* participant,
-                           unsigned char pinIn1,
-                           unsigned char pinIn2,
-                           bool reverse)
+DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
     : Thing(participant) {
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
 
 #if (ESP32)
-  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
+  in1Ch = nextAvailablePwmChannel++;
   ledcSetup(in1Ch, 500, 8);
   ledcAttachPin(pinIn1, in1Ch);
-
-  in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
+  in2Ch = nextAvailablePwmChannel++;
   ledcSetup(in2Ch, 500, 8);
   ledcAttachPin(pinIn2, in2Ch);
-
 #else
   pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
   pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
@@ -56,8 +47,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
   if (this->reverse)
     motorSpeed = -motorSpeed;
 
-  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm
-  // " << rpm
+  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
   //           << ", motor signal = " << (int)motorSignal << "\n";
 
 #if (ESP32)

Arduino/Things/DRV8833.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <Arduino.h>
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
 

EspIdf/EspIdfParticipant.cpp

@@ -7,9 +7,9 @@
 namespace RoboidControl {
 namespace EspIdf {
 
-void ParticipantUDP::Setup(int localPort,
-                           const char* remoteIpAddress,
-                           int remotePort) {
+void ParticipantUDP::Setup() {//int localPort,
+                          //  const char* remoteIpAddress,
+                          //  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);

LinearAlgebra/Matrix.cpp

@@ -1,7 +1,5 @@
 #include "Matrix.h"
-#if !defined(NO_STD) 
 #include <iostream>
-#endif
 
 namespace LinearAlgebra {
 
@@ -63,9 +61,7 @@ Matrix2::Matrix2(const Matrix2& m)
     this->data = nullptr;
   else {
     this->data = new float[this->nValues];
-
-    for (int ix = 0; ix < this->nValues; ++ix)
-      this->data[ix] = m.data[ix];
+    std::copy(m.data, m.data + nValues, this->data);
   }
 }
 
@@ -80,8 +76,7 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
       this->data = nullptr;
     else {
       this->data = new float[this->nValues];
-      for (int ix = 0; ix < this->nValues; ++ix)
-        this->data[ix] = m.data[ix];
+      std::copy(m.data, m.data + this->nValues, this->data);
     }
   }
   return *this;
@@ -94,8 +89,7 @@ Matrix2::~Matrix2() {
 
 Matrix2 Matrix2::Clone() const {
   Matrix2 r = Matrix2(this->nRows, this->nCols);
-  for (int ix = 0; ix < this->nValues; ++ix)
-    r.data[ix] = this->data[ix];
+  std::copy(this->data, this->data + this->nValues, r.data);
   return r;
 }
 
@@ -164,8 +158,8 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
 Matrix2 Matrix2::Transpose() const {
   Matrix2 r = Matrix2(this->nCols, this->nRows);
 
-  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
-    for (int colIx = 0; colIx < this->nCols; colIx++)
+  for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (uint colIx = 0; colIx < this->nCols; colIx++)
       r.data[colIx * this->nCols + rowIx] =
           this->data[rowIx * this->nCols + colIx];
   }

Messages/BinaryMsg.cpp

@@ -7,7 +7,6 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
   this->thingId = thing->id;
   this->thing = thing;
   unsigned char ix = BinaryMsg::length;
-  this->data = new char[255];
   this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }
 

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();
+  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;
 }
@@ -104,15 +104,15 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
     if (thing == nullptr)
       continue;
 
-    thing->Update(currentTimeMs, true);
-
-    if (this->isIsolated == false && thing->owner != this) {
+    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);
       this->Send(thing->owner, binaryMsg);
       delete poseMsg;
     }
+    thing->Update(currentTimeMs, true);
   }
 }
 
@@ -159,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);
@@ -176,11 +176,14 @@ 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) {
-  // std::cout << "send msg " << (int)this->buffer[0] << " to "
-  //           << remoteParticipant->ipAddress << std::endl;
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)
     return true;
@@ -227,7 +230,6 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) {
 }
 
 bool ParticipantUDP::Publish(IMessage* msg) {
-  // std::cout << "publish msg\n";
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
       static_cast<Windows::ParticipantUDP*>(this);
@@ -259,10 +261,8 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize,
   Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
   if (sender == nullptr) {
     sender = this->AddParticipant(senderIpAddress, senderPort);
-#if !defined(NO_STD)
     std::cout << "New remote participant " << sender->ipAddress << ":"
               << sender->port << std::endl;
-#endif
   }
 
   ReceiveData(packetSize, sender);
@@ -323,11 +323,9 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
     } break;
   };
 
-// Check if the buffer has been read completely
-#if !defined(NO_STD)
+  // Check if the buffer has been read completely
   if (bufferSize > 0)
     std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
-#endif
 }
 
 void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
@@ -345,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);    
   }
 }
 
@@ -389,13 +387,9 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
     thingName[nameLength] = '\0';
     thing->name = thingName;
 
-#if !defined(NO_STD)
     std::cout << thing->name;
-#endif
   }
-#if !defined(NO_STD)
   std::cout << std::endl;
-#endif
 }
 
 void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
@@ -407,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
-            << "/" << (int)msg->networkId << "]\n";
+  std::cout << this->name << ": process PoseMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "]\n";
 #endif
 }
 
@@ -421,13 +415,11 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
   Thing* thing = sender->Get(msg->thingId);
   if (thing != nullptr)
     thing->ProcessBinary(msg->data);
-#if !defined(NO_STD)
   else {
     std::cout << "  unknown thing [" << (int)msg->networkId << "/"
               << (int)msg->thingId << "]";
   }
   std::cout << std::endl;
-#endif
 }
 
 // Receive

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->ipAddress << ":" << this->owner->port << std::endl;
-  this->owner->Add(this, false);
+  this->owner->Add(this, true);
+  std::cout << "add thing [" << (int)this->id << "] to owner "
+            << this->owner->ipAddress << ":" << this->owner->port <<
+            std::endl;
 }
 
-// Thing::Thing(Participant* owner,
-//             Type thingType,
-//              int thingId) {
-//   // no participant reference yet..
-//   this->owner = owner;
-//   this->networkId = networkId;
-//   this->id = thingId;
-//   this->type = (unsigned char)thingType;
-
-//   this->linearVelocity = Spherical::zero;
-//   this->angularVelocity = Spherical::zero;
-//   // std::cout << "Created thing " << (int)this->networkId << "/" <<
-//   // (int)this->id
-//   //           << "\n";
-//   owner->Add(this, false);
-// }
+Thing::Thing(Thing* parent, int thingType, unsigned char thingId)
+    : Thing(parent->owner, thingType, thingId) {
+    this->parent = parent;
+}
 
 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
-  /// @param participant The participant for which this thing is created
-  /// @param networkId The network ID of the thing
-  /// @param thingId The ID of the thing
-  /// @param thingType The type of thing
-  // Thing(Participant* participant,
-  //       unsigned char networkId,
-  //       unsigned char thingId,
-  //       Type thingType = Type::Undetermined);
+  /// <summary>
+  /// Create a new thing as a child of another thing
+  /// </summary>
+  /// <param name="parent">The parent thing</param>
+  /// <param name="thingType">The type of thing</param>
+  /// <param name="thingId">The thing id, level at 0 to automatically generate
+  /// an id</param>
+  Thing(Thing* parent, int thingType = 0, unsigned char thingId = 0);
+
 
   /// @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