Fixes to make it work againg, but failing at udp.begin()

Arduino/ArduinoParticipant.cpp

@@ -19,12 +19,8 @@
 namespace RoboidControl {
 namespace Arduino {
 
-void ParticipantUDP::Setup(int localPort,
-                             const char* remoteIpAddress,
-                             int remotePort) {
+void ParticipantUDP::Setup() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  this->remoteIpAddress = remoteIpAddress;
-  this->remotePort = remotePort;
   GetBroadcastAddress();
 
 #if defined(UNO_R4)
@@ -38,9 +34,14 @@ void ParticipantUDP::Setup(int localPort,
     return;
   }
 #endif
-  udp.begin(localPort);
+  std::cout << "UDP.begin " << this->port << std::endl;
 
-  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
+  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";
 #endif
 }
 
@@ -112,7 +113,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   if (bufferSize <= 0)
     return true;
 
-  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
+  udp.beginPacket(this->broadcastIpAddress, this->port);
   udp.write((unsigned char*)buffer, bufferSize);
   udp.endPacket();
 

Arduino/ArduinoParticipant.h

@@ -11,15 +11,13 @@ namespace Arduino {
 
 class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
-  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Setup();
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
 #if defined(HAS_WIFI)
-  const char* remoteIpAddress = nullptr;
-  unsigned short remotePort = 0;
   char* broadcastIpAddress = nullptr;
 
   WiFiUDP udp;

Arduino/Things/DRV8833.cpp

@@ -5,18 +5,27 @@
 namespace RoboidControl {
 namespace Arduino {
 
-DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
+#if (ESP32)
+uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
+#endif
+
+DRV8833Motor::DRV8833Motor(Participant* participant,
+                           unsigned char pinIn1,
+                           unsigned char pinIn2,
+                           bool reverse)
     : Thing(participant) {
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
 
 #if (ESP32)
-  in1Ch = nextAvailablePwmChannel++;
+  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
   ledcSetup(in1Ch, 500, 8);
   ledcAttachPin(pinIn1, in1Ch);
-  in2Ch = nextAvailablePwmChannel++;
+
+  in2Ch = DRV8833Motor::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
@@ -47,7 +56,8 @@ 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,5 +1,6 @@
 #pragma once
 
+#include <Arduino.h>
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
 
@@ -16,7 +17,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 +31,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 {

LinearAlgebra/Matrix.cpp

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

Messages/BinaryMsg.cpp

@@ -7,6 +7,7 @@ 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,7 +69,7 @@ void ParticipantUDP::SetupUDP(int localPort,
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
       static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+  thisArduino->Setup();
 #elif defined(IDF_VER)
   EspIdf::ParticipantUDP* thisEspIdf =
       static_cast<EspIdf::ParticipantUDP*>(this);
@@ -255,8 +255,10 @@ 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);
@@ -318,8 +320,10 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
   };
 
 // Check if the buffer has been read completely
+#if !defined(NO_STD)
   if (bufferSize > 0)
     std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
+#endif
 }
 
 void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
@@ -353,7 +357,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
 }
 
@@ -380,9 +385,13 @@ 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) {
@@ -408,11 +417,13 @@ 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