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/ServoMotor.cpp

@@ -1,17 +1,21 @@
 #include "ServoMotor.h"
 
 #include "LinearAlgebra/FloatSingle.h"
+#include "Participant.h"
 
 namespace RoboidControl {
 
-ServoMotor::ServoMotor()
+ServoMotor::ServoMotor(Participant* participant) : Thing(participant, Thing::Servo, 0) {
-    : Thing(0) {  // for now, id should be set properly later
-  this->type = Thing::Servo;
   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);
 
@@ -106,4 +110,4 @@ void ServoMotor::SetAngle(Angle16 angle) {
   this->actualAngle = angle;
 };
 
-}
+}  // namespace RoboidControl

Things/ServoMotor.h

@@ -7,7 +7,11 @@ namespace RoboidControl {
 
 class ServoMotor : public Thing {
  public:
-  ServoMotor();
+  // Inherit constructors from Thing
+  using Thing::Thing;
+
+  ServoMotor(Participant* participant);
+  ServoMotor(Thing* parent);
 
   Direction16 rotationAxis = Direction16::up;
   Angle16 minAngle = Angle16::Degrees(-90);
@@ -44,4 +48,4 @@ class ServoMotor : public Thing {
   virtual void SetAngle(Angle16 angle);
 };
 
-}  // namespace RoboidContol
+}  // namespace RoboidControl