From 399eb5476eae99610b9883332cddbc93883b2d85 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 11 Apr 2025 17:26:17 +0200 Subject: [PATCH] Fixes --- Arduino/ArduinoParticipant.cpp | 53 ++++++++++++++++----------------- Arduino/ArduinoParticipant.h | 20 +++++++------ Arduino/ArduinoUtils.cpp | 8 ----- Arduino/Things/DRV8833.h | 11 ++++++- EspIdf/EspIdfParticipant.cpp | 6 ++-- EspIdf/EspIdfParticipant.h | 2 +- Participants/ParticipantUDP.cpp | 23 +++++++++----- Participants/ParticipantUDP.h | 2 +- Thing.cpp | 33 +++++--------------- Thing.h | 18 +++++------ Things/ServoMotor.cpp | 12 +++++--- Things/ServoMotor.h | 8 +++-- 12 files changed, 97 insertions(+), 99 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index 2b9330a..0a7640e 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -1,30 +1,32 @@ #include "ArduinoParticipant.h" -#if defined(ARDUINO) #if defined(ARDUINO_ARCH_ESP8266) +#define HAS_WIFI 1 #include #elif defined(ESP32) +#define HAS_WIFI 1 #include #elif defined(UNO_R4) +#define HAS_WIFI 1 #include #elif defined(ARDUINO_ARCH_RP2040) // not functional, for future use +#define HAS_WIFI 1 #include -#endif #endif namespace RoboidControl { namespace Arduino { -void ParticipantUDP::Setup(int localPort, - const char* remoteIpAddress, - int remotePort) { +#if HAS_WIFI +WiFiUDP udp; +#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, - // 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(); } -#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; }; diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 94196d7..c013486 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -2,28 +2,30 @@ #include "Participants/ParticipantUDP.h" -#if defined(HAS_WIFI) -#include -#endif +// #if defined(ARDUINO_ARCH_ESP8266) || defined(ESP32) || defined(UNO_R4) || \ +// defined(ARDUINO_ARCH_RP2040) +// #define HAS_WIFI 1 +// #endif + +// #if defined(HAS_WIFI) +// #include +// #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) - const char* remoteIpAddress = nullptr; - unsigned short remotePort = 0; +//#if defined(HAS_WIFI) char* broadcastIpAddress = nullptr; - WiFiUDP udp; -#endif +//#endif void GetBroadcastAddress(); }; diff --git a/Arduino/ArduinoUtils.cpp b/Arduino/ArduinoUtils.cpp index eaf4ccc..43da5a5 100644 --- a/Arduino/ArduinoUtils.cpp +++ b/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 \ No newline at end of file diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index cbbfb3c..d330877 100644 --- a/Arduino/Things/DRV8833.h +++ b/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 { diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 22e8160..5dea2ac 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/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(); diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index cf1c6bf..869d451 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/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); diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index f643587..ed002ef 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -69,11 +69,11 @@ void ParticipantUDP::SetupUDP(int localPort, #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = static_cast(this); - thisArduino->Setup(localPort, remoteIpAddress, remotePort); + thisArduino->Setup(); // localPort, remoteIpAddress, remotePort); #elif defined(IDF_VER) EspIdf::ParticipantUDP* thisEspIdf = static_cast(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 - << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process PoseMsg [" << (int)msg->networkId + << "/" << (int)msg->thingId << "]\n"; #endif } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index ec93af9..b7e2109 100644 --- a/Participants/ParticipantUDP.h +++ b/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); diff --git a/Thing.cpp b/Thing.cpp index 86e4008..3555da2 100644 --- a/Thing.cpp +++ b/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); diff --git a/Thing.h b/Thing.h index febe7a9..46a1ec2 100644 --- a/Thing.h +++ b/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); + /// + /// Create a new thing as a child of another thing + /// + /// The parent thing + /// The type of thing + /// The thing id, level at 0 to automatically generate + /// an id + Thing(Thing* parent, int thingType = 0, unsigned char thingId = 0); + /// @brief The participant managing this thing Participant* owner; diff --git a/Things/ServoMotor.cpp b/Things/ServoMotor.cpp index b8f111d..79e0212 100644 --- a/Things/ServoMotor.cpp +++ b/Things/ServoMotor.cpp @@ -1,17 +1,21 @@ #include "ServoMotor.h" #include "LinearAlgebra/FloatSingle.h" +#include "Participant.h" namespace RoboidControl { -ServoMotor::ServoMotor() - : Thing(0) { // for now, id should be set properly later - this->type = Thing::Servo; +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); @@ -106,4 +110,4 @@ void ServoMotor::SetAngle(Angle16 angle) { this->actualAngle = angle; }; -} \ No newline at end of file +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/ServoMotor.h b/Things/ServoMotor.h index 3170543..8c00cbc 100644 --- a/Things/ServoMotor.h +++ b/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