Merge commit 'a6a91798b24552522373414d43981b082025cfdd' into ControlCore

Arduino/ArduinoParticipant.cpp

@@ -19,8 +19,10 @@
 namespace RoboidControl {
 namespace Arduino {
 
-void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void LocalParticipant::Setup(int localPort,
-#if defined(ARDUINO)
+                             const char* remoteIpAddress,
+                             int remotePort) {
+#if defined(ARDUINO) && defined(HAS_WIFI)
   this->remoteIpAddress = remoteIpAddress;
   this->remotePort = remotePort;
   GetBroadcastAddress();
@@ -43,18 +45,19 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
 }
 
 void LocalParticipant::GetBroadcastAddress() {
-#if defined(ARDUINO)
+#if defined(ARDUINO) && defined(HAS_WIFI)
   IPAddress broadcastAddress = WiFi.localIP();
   broadcastAddress[3] = 255;
   String broadcastIpString = broadcastAddress.toString();
   this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
-  broadcastIpString.toCharArray(this->broadcastIpAddress, broadcastIpString.length() + 1);
+  broadcastIpString.toCharArray(this->broadcastIpAddress,
+                                broadcastIpString.length() + 1);
   std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
 #endif
 }
 
 void LocalParticipant::Receive() {
-#if defined(ARDUINO)
+#if defined(ARDUINO) && defined(HAS_WIFI)
   int packetSize = udp.parsePacket();
   while (packetSize > 0) {
     udp.read(buffer, packetSize);
@@ -64,12 +67,14 @@ void LocalParticipant::Receive() {
     senderAddress.toCharArray(sender_ipAddress, 16);
     unsigned int sender_port = udp.remotePort();
 
-    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
-    // if (remoteParticipant == nullptr) {
+    // sender_port); if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->AddParticipant(sender_ipAddress,
+    //   sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
     //   //           << "\n";
-    //   // std::cout << "New remote participant " << remoteParticipant->ipAddress
+    //   // std::cout << "New remote participant " <<
+    //   remoteParticipant->ipAddress
     //   //           << ":" << remoteParticipant->port << " "
     //   //           << (int)remoteParticipant->networkId << "\n";
     // }
@@ -82,7 +87,7 @@ void LocalParticipant::Receive() {
 }
 
 bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
-#if defined(ARDUINO)
+#if defined(ARDUINO) && defined(HAS_WIFI)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
   // << remoteParticipant->port << "\n";
 
@@ -102,7 +107,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
 }
 
 bool LocalParticipant::Publish(IMessage* msg) {
-#ifdef ARDUINO
+#if defined(ARDUINO) && defined(HAS_WIFI)
   int bufferSize = msg->Serialize((char*)this->buffer);
   if (bufferSize <= 0)
     return true;

Arduino/ArduinoParticipant.h

@@ -2,6 +2,10 @@
 
 #include "../LocalParticipant.h"
 
+#if defined(HAS_WIFI)
+#include <WiFiUdp.h>
+#endif
+
 namespace RoboidControl {
 namespace Arduino {
 
@@ -13,6 +17,14 @@ class LocalParticipant : public RoboidControl::LocalParticipant {
   bool Publish(IMessage* msg);
 
  protected:
+#if defined(HAS_WIFI)
+  const char* remoteIpAddress = nullptr;
+  unsigned short remotePort = 0;
+  char* broadcastIpAddress = nullptr;
+
+  WiFiUDP udp;
+#endif
+
   void GetBroadcastAddress();
 };
 

Arduino/ArduinoUtils.cpp

@@ -42,8 +42,13 @@ struct NssServer {
 } nssServer;
 #endif
 
-bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallback) {
+bool StartWifi(const char* wifiSsid,
-#if UNO_R4 || ARDUINO_ARCH_RP2040
+               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");
     return false;
@@ -120,8 +125,10 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
 #if ESP32
     printf("Checking credentials in flash\n");
     wifiPreferences.begin(PREFERENCES_NAMESPACE);
-    wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials));
+    wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials,
-    if (strcmp(wifiSsid, credentials.ssid) != 0 || strcmp(wifiPassword, credentials.password) != 0) {
+                             sizeof(credentials));
+    if (strcmp(wifiSsid, credentials.ssid) != 0 ||
+        strcmp(wifiPassword, credentials.password) != 0) {
       printf("Updating credentials in flash...");
       const int ssidLen = strlen(wifiSsid);
       if (ssidLen < 32) {
@@ -134,7 +141,8 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
         memcpy(credentials.password, wifiPassword, pwdLen);
         credentials.password[pwdLen] = '\0';
       }
-      wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials));
+      wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials,
+                               sizeof(credentials));
       printf(" completed.\n");
     }
     wifiPreferences.end();
@@ -142,10 +150,15 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
   }
 
   return (!hotSpotEnabled);
+#endif
 }
 
 void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
-#if defined(UNO_R4)  // Uno R4 Wifi does not support this kind of firmware update (as far as I know)
+#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;
 #else
   Serial.println("Checking for firmware updates.");
@@ -177,10 +190,12 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
       switch (ret) {
         case HTTP_UPDATE_FAILED:
 #if defined(ESP32)
-          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s", httpUpdate.getLastError(),
+          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
+                        httpUpdate.getLastError(),
                         httpUpdate.getLastErrorString().c_str());
 #else
-          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s", ESPhttpUpdate.getLastError(),
+          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
+                        ESPhttpUpdate.getLastError(),
                         ESPhttpUpdate.getLastErrorString().c_str());
 #endif
           break;
@@ -198,5 +213,6 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
     Serial.println(httpCode);
   }
 #endif
+#endif
 }
 #endif

Arduino/Things/DRV8833.cpp

@@ -3,11 +3,9 @@
 #include <Arduino.h>
 
 namespace RoboidControl {
+namespace Arduino {
 
-DRV8833Motor::DRV8833Motor(Participant* participant,
+DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
-                           unsigned char pinIn1,
-                           unsigned char pinIn2,
-                           bool reverse)
     : Thing(participant) {
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
@@ -118,4 +116,5 @@ DRV8833::DRV8833(Participant* participant,
   this->motorB->name = "Motor B";
 }
 
+}  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.cpp

@@ -5,16 +5,18 @@
 namespace RoboidControl {
 namespace Arduino {
 
-DigitalInput::DigitalInput(Participant* participant, unsigned char pin) : TouchSensor(participant) {
+DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
+    : TouchSensor(participant) {
   this->pin = pin;
 
   pinMode(pin, INPUT);
 }
 
-void DigitalInput::Update(unsigned long currentTimeMs) {
+void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
   this->touchedSomething = digitalRead(pin) == LOW;
-
+  // std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
-    // std::cout << "DigitalINput pin " << (int)this->pin << ": " << this->touchedSomething << "\n";
+  // this->touchedSomething << "\n";
+  Thing::Update(currentTimeMs, recursive);
 }
 
 }  // namespace Arduino

Arduino/Things/DigitalInput.h

@@ -14,7 +14,8 @@ class DigitalInput : public TouchSensor {
   DigitalInput(Participant* participant, unsigned char pin);
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(unsigned long currentTimeMs) override;
+  virtual void Update(unsigned long currentTimeMs,
+                      bool recursive = false) override;
 
  protected:
   /// @brief The pin used for digital input

Arduino/Things/UltrasonicSensor.cpp

@@ -5,7 +5,9 @@
 namespace RoboidControl {
 namespace Arduino {
 
-UltrasonicSensor::UltrasonicSensor(Participant* participant, unsigned char pinTrigger, unsigned char pinEcho)
+UltrasonicSensor::UltrasonicSensor(Participant* participant,
+                                   unsigned char pinTrigger,
+                                   unsigned char pinEcho)
     : TouchSensor(participant) {
   this->pinTrigger = pinTrigger;
   this->pinEcho = pinEcho;
@@ -23,7 +25,8 @@ float UltrasonicSensor::GetDistance() {
   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
+  float duration_us =
+      pulseIn(pinEcho, HIGH, 100000);  // the result is in microseconds
 
   // Calculate the distance:
   // * Duration should be divided by 2, because the ping goes to the object
@@ -43,14 +46,16 @@ float UltrasonicSensor::GetDistance() {
 
   this->touchedSomething |= (this->distance <= this->touchDistance);
 
-//   std::cout << "Ultrasonic " << this->distance << " " << this->touchedSomething << "\n";
+  //   std::cout << "Ultrasonic " << this->distance << " " <<
+  //   this->touchedSomething << "\n";
 
   return distance;
 }
 
-void UltrasonicSensor::Update(unsigned long currentTimeMs) {
+void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
   this->touchedSomething = false;
   GetDistance();
+  Thing::Update(currentTimeMs, recursive);
 }
 
 // void UltrasonicSensor::ProcessBinary(char* bytes) {

Arduino/Things/UltrasonicSensor.h

@@ -12,7 +12,9 @@ class UltrasonicSensor : public TouchSensor {
   /// @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(Participant* participant,
+                   unsigned char pinTrigger,
+                   unsigned char pinEcho);
 
   // parameters
 
@@ -23,12 +25,14 @@ class UltrasonicSensor : public TouchSensor {
 
   /// @brief The last read distance
   float distance = 0;
-  /// @brief erform an ultrasonic 'ping' to determine the distance to the nearest object
+  /// @brief erform an ultrasonic 'ping' to determine the distance to the
+  /// nearest object
   /// @return the measured distance in meters to the nearest object
   float GetDistance();
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(unsigned long currentTimeMs) override;
+  virtual void Update(unsigned long currentTimeMs,
+                      bool recursive = false) override;
 
  protected:
   /// @brief The pin number of the trigger signal

Examples/BB2B.cpp

@@ -2,12 +2,18 @@
 #include "Things/DifferentialDrive.h"
 #include "Things/TouchSensor.h"
 
+#if defined(ARDUINO)
+#include "Arduino.h"
+
+#else
 #include <chrono>
 #include <thread>
 
-using namespace RoboidControl;
 using namespace std::this_thread;
 using namespace std::chrono;
+#endif
+
+using namespace RoboidControl;
 
 int main() {
   // The robot's propulsion is a differential drive
@@ -33,7 +39,11 @@ int main() {
     bb2b->Update(true);
 
     // and sleep for 100ms
+#if defined(ARDUINO)
+    delay(100);
+#else
     sleep_for(milliseconds(100));
+#endif
   }
 
   return 0;

LocalParticipant.cpp

@@ -5,21 +5,23 @@
 #include "Arduino/ArduinoParticipant.h"
 
 #if defined(_WIN32) || defined(_WIN64)
-#include "Windows/WindowsParticipant.h"
 #include <winsock2.h>
 #include <ws2tcpip.h>
+#include "Windows/WindowsParticipant.h"
 #pragma comment(lib, "ws2_32.lib")
 
 #elif defined(__unix__) || defined(__APPLE__)
-#include "Posix/PosixParticipant.h"
 #include <arpa/inet.h>
 #include <fcntl.h>  // For fcntl
 #include <netinet/in.h>
 #include <sys/socket.h>
 #include <unistd.h>
 #include <chrono>
+#include "Posix/PosixParticipant.h"
 #endif
 
+#include <string.h>
+
 namespace RoboidControl {
 
 // LocalParticipant::LocalParticipant() {}
@@ -32,7 +34,8 @@ LocalParticipant::LocalParticipant(int port) {
 }
 
 LocalParticipant::LocalParticipant(const char* ipAddress, int port) {
-  this->ipAddress = "0.0.0.0";  // ipAddress;  // maybe this is not needed anymore, keeping it to "0.0.0.0"
+  this->ipAddress = "0.0.0.0";  // ipAddress;  // maybe this is not needed
+                                // anymore, keeping it to "0.0.0.0"
   this->port = port;
   if (this->port == 0)
     this->isIsolated = true;
@@ -55,15 +58,20 @@ void LocalParticipant::begin() {
   SetupUDP(this->port, this->ipAddress, this->port);
 }
 
-void LocalParticipant::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) {
+void LocalParticipant::SetupUDP(int localPort,
+                                const char* remoteIpAddress,
+                                int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  Windows::LocalParticipant* thisWindows =
+      static_cast<Windows::LocalParticipant*>(this);
   thisWindows->Setup(localPort, remoteIpAddress, remotePort);
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  Posix::LocalParticipant* thisPosix =
+      static_cast<Posix::LocalParticipant*>(this);
   thisPosix->Setup(localPort, remoteIpAddress, remotePort);
 #elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  Arduino::LocalParticipant* thisArduino =
+      static_cast<Arduino::LocalParticipant*>(this);
   thisArduino->Setup(localPort, remoteIpAddress, remotePort);
 #endif
   this->connected = true;
@@ -71,13 +79,15 @@ void LocalParticipant::SetupUDP(int localPort, const char* remoteIpAddress, int
 
 void LocalParticipant::Update(unsigned long currentTimeMs) {
   if (currentTimeMs == 0) {
-#if defined(ARDUINO)
+    currentTimeMs = Thing::GetTimeMs();
-    currentTimeMs = millis();
+    // #if defined(ARDUINO)
-#elif defined(__unix__) || defined(__APPLE__)
+    //     currentTimeMs = millis();
-    auto now = std::chrono::steady_clock::now();
+    // #elif defined(__unix__) || defined(__APPLE__)
-    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
+    //     auto now = std::chrono::steady_clock::now();
-    currentTimeMs = static_cast<unsigned long>(ms.count());
+    //     auto ms =
-#endif
+    //     std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
+    //     currentTimeMs = static_cast<unsigned long>(ms.count());
+    // #endif
   }
 
   if (this->isIsolated == false) {
@@ -112,13 +122,16 @@ void LocalParticipant::Update(unsigned long currentTimeMs) {
 
 void LocalParticipant::ReceiveUDP() {
 #if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  Windows::LocalParticipant* thisWindows =
+      static_cast<Windows::LocalParticipant*>(this);
   thisWindows->Receive();
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  Posix::LocalParticipant* thisPosix =
+      static_cast<Posix::LocalParticipant*>(this);
   thisPosix->Receive();
 #elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  Arduino::LocalParticipant* thisArduino =
+      static_cast<Arduino::LocalParticipant*>(this);
   thisArduino->Receive();
 #endif
 }
@@ -132,17 +145,23 @@ Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) {
 }
 
 Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) {
-  std::cout << "New Participant " << ipAddress << ":" << port << "\n";
+  // std::cout << "New Participant " << ipAddress << ":" << port << "\n";
   Participant* participant = new Participant(ipAddress, port);
+#if defined(NO_STD)
+  participant->networkId = this->senderCount;
+  this->senders[this->senderCount++] = participant;
+#else
   participant->networkId = (unsigned char)this->senders.size();
   this->senders.push_back(participant);
+#endif
   return participant;
 }
 
 #pragma region Send
 
-void LocalParticipant::SendThingInfo(Participant* remoteParticipant, Thing* thing) {
+void LocalParticipant::SendThingInfo(Participant* remoteParticipant,
-  std::cout << "Send thing info " << (int)thing->id << " \n";
+                                     Thing* thing) {
+  // std::cout << "Send thing info " << (int)thing->id << " \n";
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
   this->Send(remoteParticipant, thingMsg);
   delete thingMsg;
@@ -166,13 +185,16 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
     return true;
 
 #if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  Windows::LocalParticipant* thisWindows =
+      static_cast<Windows::LocalParticipant*>(this);
   return thisWindows->Send(remoteParticipant, bufferSize);
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  Posix::LocalParticipant* thisPosix =
+      static_cast<Posix::LocalParticipant*>(this);
   return thisPosix->Send(remoteParticipant, bufferSize);
 #elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  Arduino::LocalParticipant* thisArduino =
+      static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Send(remoteParticipant, bufferSize);
 #endif
 }
@@ -200,13 +222,16 @@ void LocalParticipant::PublishThingInfo(Thing* thing) {
 
 bool LocalParticipant::Publish(IMessage* msg) {
 #if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  Windows::LocalParticipant* thisWindows =
+      static_cast<Windows::LocalParticipant*>(this);
   return thisWindows->Publish(msg);
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  Posix::LocalParticipant* thisPosix =
+      static_cast<Posix::LocalParticipant*>(this);
   return thisPosix->Publish(msg);
 #elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  Arduino::LocalParticipant* thisArduino =
+      static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Publish(msg);
 #endif
 }
@@ -216,8 +241,11 @@ bool LocalParticipant::Publish(IMessage* msg) {
 
 #pragma region Receive
 
-void LocalParticipant::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) {
+void LocalParticipant::ReceiveData(unsigned char packetSize,
-  Participant* remoteParticipant = this->GetParticipant(senderIpAddress, senderPort);
+                                   char* senderIpAddress,
+                                   unsigned int senderPort) {
+  Participant* remoteParticipant =
+      this->GetParticipant(senderIpAddress, senderPort);
   if (remoteParticipant == nullptr) {
     remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
     // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
@@ -230,7 +258,8 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, char* senderIpAddre
   ReceiveData(packetSize, remoteParticipant);
 }
 
-void LocalParticipant::ReceiveData(unsigned char bufferSize, Participant* remoteParticipant) {
+void LocalParticipant::ReceiveData(unsigned char bufferSize,
+                                   Participant* remoteParticipant) {
   unsigned char msgId = this->buffer[0];
   // std::cout << "receive msg " << (int)msgId << "\n";
   switch (msgId) {
@@ -275,10 +304,11 @@ void LocalParticipant::ReceiveData(unsigned char bufferSize, Participant* remote
 void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {}
 
 void LocalParticipant::Process(Participant* sender, SiteMsg* msg) {
-  std::cout << this->name << ": process NetworkId [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n";
+  // std::cout << this->name << ": process NetworkId [" << (int)this->networkId
+  //           << "/" << (int)msg->networkId << "]\n";
   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);
   }
@@ -295,22 +325,27 @@ void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
     int stringLen = nameLength + 1;
     char* thingName = new char[stringLen];
 #if defined(_WIN32) || defined(_WIN64)
-    strncpy_s(thingName, stringLen, msg->name, stringLen - 1);  // Leave space for null terminator
+    strncpy_s(thingName, stringLen, msg->name,
+              stringLen - 1);  // Leave space for null terminator
 #else
-    // Use strncpy with bounds checking for other platforms (Arduino, POSIX, ESP-IDF)
+    // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
-    strncpy(thingName, msg->name, stringLen - 1);  // Leave space for null terminator
+    // ESP-IDF)
+    strncpy(thingName, msg->name,
+            stringLen - 1);           // Leave space for null terminator
     thingName[stringLen - 1] = '\0';  // Ensure null termination
 #endif
     thingName[nameLength] = '\0';
     thing->name = thingName;
-    std::cout << "thing name = " << thing->name << " length = " << nameLength << "\n";
+    // std::cout << "thing name = " << thing->name << " length = " << nameLength
+    //           << "\n";
   }
 }
 
 void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {}
 
 void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
-  // std::cout << this->name << ": process Binary [" << (int)this->networkId << "/"
+  // std::cout << this->name << ": process Binary [" << (int)this->networkId <<
+  // "/"
   //           << (int)msg->networkId << "]\n";
   Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr)
@@ -319,8 +354,9 @@ void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
     thing = this->Get(msg->networkId, msg->thingId);
     if (thing != nullptr)
       thing->ProcessBinary(msg->bytes);
-    else
+    // else
-      std::cout << "custom msg for unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n";
+    //   std::cout << "custom msg for unknown thing [" << (int)msg->networkId
+    //             << "/" << (int)msg->thingId << "]\n";
   }
 }
 

LocalParticipant.h

@@ -10,7 +10,9 @@
 #include "Messages/ThingMsg.h"
 #include "Participant.h"
 
+#if !defined(NO_STD)
 #include <list>
+#endif
 
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@@ -20,34 +22,41 @@
 #include <sys/socket.h>
 #include <unistd.h>
 #elif defined(ARDUINO)
-#include <WiFiUdp.h>
+// #include <WiFiUdp.h>
 #endif
 
 namespace RoboidControl {
 
-/// @brief A local participant is the local device which can communicate with other participants
+constexpr int MAX_SENDER_COUNT = 256;
-/// It manages all local things and communcation with other participants.
+
-/// Each application has a local participant which is usually explicit in the code.
+/// @brief A local participant is the local device which can communicate with
-/// An participant can be isolated. In that case it is standalong and does not communicate with other participants.
+/// other participants It manages all local things and communcation with other
+/// participants. Each application has a local participant which is usually
+/// explicit in the code. An participant can be isolated. In that case it is
+/// standalong and does not communicate with other participants.
 ///
-/// It is possible to work with an hidden participant by creating things without specifying a participant in the
+/// It is possible to work with an hidden participant by creating things without
-/// constructor. In that case an hidden isolated participant is created which can be
+/// specifying a participant in the constructor. In that case an hidden isolated
-/// obtained using RoboidControl::LocalParticipant::Isolated().
+/// participant is created which can be obtained using
+/// RoboidControl::LocalParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
 class LocalParticipant : public Participant {
  public:
   /// @brief Create a participant without connecting to a site
   /// @param port The port on which the participant communicates
-  /// These participant typically broadcast Participant messages to let site servers on the local network know their presence.
+  /// These participant typically broadcast Participant messages to let site
-  /// Alternatively they can broadcast information which can be used directly by other participants.
+  /// servers on the local network know their presence. Alternatively they can
+  /// broadcast information which can be used directly by other participants.
   LocalParticipant(int port = 7681);
   /// @brief Create a participant which will try to connect to a site.
   /// @param ipAddress The IP address of the site
   /// @param port The port used by the site
   LocalParticipant(const char* ipAddress, int port = 7681);
-  // Note to self: one cannot specify the port used by the local participant now!!
+  // Note to self: one cannot specify the port used by the local participant
+  // now!!
 
-  /// @brief Isolated participant is used when the application is run without networking
+  /// @brief Isolated participant is used when the application is run without
+  /// networking
   /// @return A participant without networking support
   static LocalParticipant* Isolated();
 
@@ -55,7 +64,8 @@ class LocalParticipant : public Participant {
   /// Isolated participants do not communicate with other participants
   bool isIsolated = false;
 
-  /// The interval in milliseconds for publishing (broadcasting) data on the local network
+  /// The interval in milliseconds for publishing (broadcasting) data on the
+  /// local network
   long publishInterval = 3000;  // 3 seconds
 
   /// @brief The name of the participant
@@ -67,11 +77,11 @@ class LocalParticipant : public Participant {
   Participant* remoteSite = nullptr;
 
 #if defined(ARDUINO)
-  const char* remoteIpAddress = nullptr;
+  // const char* remoteIpAddress = nullptr;
-  unsigned short remotePort = 0;
+  // unsigned short remotePort = 0;
-  char* broadcastIpAddress = nullptr;
+  // char* broadcastIpAddress = nullptr;
 
-  WiFiUDP udp;
+  // WiFiUDP udp;
 #else
 
 #if defined(__unix__) || defined(__APPLE__)
@@ -94,10 +104,17 @@ class LocalParticipant : public Participant {
   bool Send(Participant* remoteParticipant, IMessage* msg);
   bool Publish(IMessage* msg);
 
-  void ReceiveData(unsigned char bufferSize, char* senderIpAddress, unsigned int senderPort);
+  void ReceiveData(unsigned char bufferSize,
+                   char* senderIpAddress,
+                   unsigned int senderPort);
   void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
 
+#if defined(NO_STD)
+  unsigned char senderCount = 0;
+  Participant* senders[MAX_SENDER_COUNT];
+#else
   std::list<Participant*> senders;
+#endif
 
  protected:
   unsigned long nextPublishMe = 0;

Messages/LowLevelMessages.cpp

@@ -1,6 +1,6 @@
 #include "LowLevelMessages.h"
 
-#include <iostream>
+// #include <iostream>
 #include "LinearAlgebra/float16.h"
 
 namespace RoboidControl {

Participant.cpp

@@ -42,15 +42,24 @@ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
 void Participant::Add(Thing* thing, bool checkId) {
   if (checkId && thing->id == 0) {
     // allocate a new thing ID
+#if defined(NO_STD)
+    thing->id = this->thingCount + 1;
+    this->things[this->thingCount++] = thing;
+#else
     thing->id = (unsigned char)this->things.size() + 1;
     this->things.push_back(thing);
+#endif
     // std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
     // this->port << "[" << (int)thing->networkId << "/"
     //           << (int)thing->id << "]\n";
   } else {
     Thing* foundThing = Get(thing->networkId, thing->id);
     if (foundThing == nullptr) {
+#if defined(NO_STD)
+      this->things[this->thingCount++] = thing;
+#else
       this->things.push_back(thing);
+#endif
       // std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
       // "[" << (int)thing->networkId << "/"
       //           << (int)thing->id << "]\n";
@@ -63,9 +72,24 @@ void Participant::Add(Thing* thing, bool checkId) {
 }
 
 void Participant::Remove(Thing* thing) {
+#if defined(NO_STD)
+  for (unsigned char thingIx = 0; thingIx < this->thingCount; thingIx++)
+    if (this->things[thingIx] == thing)
+      this->things[thingIx] = nullptr;
+  // compacting
+  unsigned char lastThingIx = 0;
+  for (unsigned char thingIx = 0; thingIx < this->thingCount; thingIx++) {
+    if (this->things[thingIx] == nullptr)
+      continue;
+    this->things[lastThingIx] = this->things[thingIx];
+    lastThingIx++;
+  }
+  this->thingCount = lastThingIx;
+#else
   this->things.remove_if([thing](Thing* obj) { return obj == thing; });
   std::cout << "Removing " << thing->networkId << "/" << thing->id
             << " list size = " << this->things.size() << "\n";
+#endif
 }
 
 // void Participant::UpdateAll(unsigned long currentTimeMs) {

Participant.h

@@ -3,16 +3,21 @@
 
 namespace RoboidControl {
 
+constexpr int MAX_THING_COUNT = 256;
+
 /// @brief A participant is a device which manages things.
-/// It can communicate with other participant to synchronise the state of things.
+/// It can communicate with other participant to synchronise the state of
-/// This class is used to register the things the participant is managing.
+/// things. This class is used to register the things the participant is
-/// It also maintains the communcation information to contact the participant.
+/// managing. It also maintains the communcation information to contact the
-/// It is used as a basis for the local participant, but also as a reference to remote participants.
+/// participant. It is used as a basis for the local participant, but also as a
+/// reference to remote participants.
 class Participant {
  public:
-  /// @brief The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
+  /// @brief The Ip Address of a participant. When the participant is local,
+  /// this contains 0.0.0.0
   const char* ipAddress = "0.0.0.0";
-  /// @brief The port number for UDP communication with the participant. This is 0 for isolated participants.
+  /// @brief The port number for UDP communication with the participant. This is
+  /// 0 for isolated participants.
   int port = 0;
 
   /// @brief The network Id to identify the participant.
@@ -29,8 +34,13 @@ public:
   ~Participant();
 
  protected:
+#if defined(NO_STD)
+  unsigned char thingCount = 0;
+  Thing* things[MAX_THING_COUNT];
+#else
   /// @brief The list of things managed by this participant
   std::list<Thing*> things;
+#endif
 
  public:
   /// @brief Find a thing managed by this participant
@@ -41,12 +51,12 @@ public:
   Thing* Get(unsigned char networkId, unsigned char thingId);
   /// @brief Add a new thing for this participant.
   /// @param thing The thing to add
-  /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing Id will be assigned.
+  /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
+  /// Id will be assigned.
   void Add(Thing* thing, bool checkId = true);
   /// @brief Remove a thing for this participant
   /// @param thing The thing to remove
   void Remove(Thing* thing);
-
 };
 
-} // namespace Control
+}  // namespace RoboidControl

README.md

@@ -9,6 +9,9 @@ Supporting:
     - ESP8266
     - ESP32
     - UNO R4 WiFi
+    - ESP8266
+    - ESP32
+    - UNO R4 WiFi
 
 # Basic components
 

SiteServer.cpp

@@ -2,8 +2,10 @@
 
 #include "Things/TemperatureSensor.h"
 
+#if !defined(NO_STD)
 #include <functional>
 #include <memory>
+#endif
 
 namespace RoboidControl {
 
@@ -14,17 +16,24 @@ SiteServer::SiteServer(int port) {
   this->ipAddress = "0.0.0.0";
   this->port = port;
 
+#if defined(NO_STD)
+  this->senders[this->senderCount++] = this;
+#else
   this->senders.push_back(this);
+#endif
 
   SetupUDP(port, ipAddress, 0);
 
+#if !defined(NO_STD)
   Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
+#endif
 }
 
 void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
   if (msg->networkId == 0) {
-    std::cout << this->name << " received New Client -> " << sender->ipAddress
+    // std::cout << this->name << " received New Client -> " <<
-              << ":" << (int)sender->port << "\n";
+    // sender->ipAddress
+    //           << ":" << (int)sender->port << "\n";
     SiteMsg* msg = new SiteMsg(sender->networkId);
     this->Send(sender, msg);
     delete msg;
@@ -36,15 +45,20 @@ void SiteServer::Process(Participant *sender, SiteMsg *msg) {}
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing == nullptr) {
+#if defined(NO_STD)
+    new Thing(sender, msg->networkId, msg->thingId,
+              (Thing::Type)msg->thingType);
+#else
     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
     Thing* newThing;
     if (thingMsgProcessor != thingMsgProcessors.end())  // found item
-      newThing = thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
+      newThing =
+          thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
     else
       newThing = new Thing(sender, msg->networkId, msg->thingId,
                            (Thing::Type)msg->thingType);
-    //sender->Add(newThing);
+#endif
   }
 }
 
-} // namespace Control
+}  // namespace RoboidControl

SiteServer.h

@@ -2,9 +2,11 @@
 
 #include "LocalParticipant.h"
 
+#if !defined(NO_STD)
 #include <functional>
 #include <memory>
 #include <unordered_map>
+#endif
 
 namespace RoboidControl {
 
@@ -15,12 +17,16 @@ class SiteServer : public LocalParticipant {
 
   // virtual void Update(unsigned long currentTimeMs = 0) override;
 
+#if !defined(NO_STD)
   template <typename ThingClass>
   void Register(unsigned char thingType) {
-    thingMsgProcessors[thingType] = [](Participant* participant, unsigned char networkId, unsigned char thingId) {
+    thingMsgProcessors[thingType] = [](Participant* participant,
+                                       unsigned char networkId,
+                                       unsigned char thingId) {
       return new ThingClass(participant, networkId, thingId);
     };
   };
+#endif
 
  protected:
   unsigned long nextPublishMe = 0;
@@ -29,8 +35,12 @@ class SiteServer : public LocalParticipant {
   virtual void Process(Participant* sender, SiteMsg* msg) override;
   virtual void Process(Participant* sender, ThingMsg* msg) override;
 
-  using ThingConstructor = std::function<Thing*(Participant* participant, unsigned char networkId, unsigned char thingId)>;
+#if !defined(NO_STD)
+  using ThingConstructor = std::function<Thing*(Participant* participant,
+                                                unsigned char networkId,
+                                                unsigned char thingId)>;
   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
+#endif
 };
 
 }  // namespace RoboidControl

Thing.cpp

@@ -1,13 +1,17 @@
 #include "Thing.h"
 
-#include <string.h>
-#include <algorithm>
-#include <chrono>
-#include <iostream>
-#include <list>
-
 #include "LocalParticipant.h"
 
+#include <string.h>
+// #include <algorithm>
+// #include <iostream>
+// #include <list>
+// #include <chrono>
+
+#if defined(ARDUINO)
+#include "Arduino.h"
+#endif
+
 namespace RoboidControl {
 
 // LocalParticipant* Thing::CheckHiddenParticipant() {
@@ -169,18 +173,18 @@ void Thing::SetModel(const char* url) {
 }
 
 unsigned long Thing::GetTimeMs() {
+#if defined(ARDUINO)
+  return millis();
+#else
   auto now = std::chrono::steady_clock::now();
   auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
       now.time_since_epoch());
   return static_cast<unsigned long>(ms.count());
+#endif
 }
 
 void Thing::Update(bool recursive) {
-#if defined(ARDUINO)
-  Update(millis());
-#else
   Update(GetTimeMs(), recursive);
-#endif
 }
 
 void Thing::Update(unsigned long currentTimeMs, bool recursive) {

Thing.h

@@ -1,6 +1,9 @@
 #pragma once
+
+#if !defined(NO_STD)
 #include <iostream>
 #include <list>
+#endif
 #include "LinearAlgebra/Spherical.h"
 #include "LinearAlgebra/SwingTwist.h"
 

Things/DifferentialDrive.cpp

@@ -24,6 +24,7 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
 void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
   float distance = this->wheelSeparation / 2;
   this->leftWheel = leftWheel;
+  ;
   if (leftWheel != nullptr)
     this->leftWheel->SetPosition(Spherical(distance, Direction::left));
 

Things/TemperatureSensor.cpp

@@ -8,7 +8,9 @@ namespace RoboidControl {
 
 // TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
 
-TemperatureSensor::TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId)
+TemperatureSensor::TemperatureSensor(Participant* participant,
+                                     unsigned char networkId,
+                                     unsigned char thingId)
     : Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
 
 void TemperatureSensor::SetTemperature(float temp) {
@@ -16,7 +18,7 @@ void TemperatureSensor::SetTemperature(float temp) {
 }
 
 void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
-  std::cout << "Send temperature: " << this->temperature << "\n";
+  // std::cout << "Send temperature: " << this->temperature << "\n";
   LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
 }
 

Things/TouchSensor.cpp

@@ -13,8 +13,8 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
 void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
 
 void TouchSensor::ProcessBinary(char* bytes) {
-  if (bytes[0] == 1)
+  // if (bytes[0] == 1)
-    std::cout << this->name << " is Touching something!\n";
+  //   std::cout << this->name << " is Touching something!\n";
   this->touchedSomething |= (bytes[0] == 1);
 }
 

library.json

@@ -0,0 +1,20 @@
+{
+    "name": "RoboidCOntrol",
+    "version": "0.3.0",
+    "description": "Controlling Roboids",
+    "keywords": "robotics, networking",
+    "authors": [
+        {
+            "name": "Pascal Serrarens",
+            "email": "pascal@passer.life"
+        }
+    ],
+    "license": "MPL",
+    "src": {
+        "include": "Thing.h",
+        "src": [
+            "Things/*.cpp",
+            "Arduino/*.cpp"
+        ]
+    }
+}