RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixes

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-11 17:26:17 +02:00
parent 467ffd8d07
commit 399eb5476e
12 changed files with 97 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
Arduino/ArduinoParticipant.cpp
View File

@ -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,
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,23 +73,13 @@ 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) {
@ -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;
};

20
Arduino/ArduinoParticipant.h
View File

@ -2,28 +2,30 @@
#include "Participants/ParticipantUDP.h"
#if defined(HAS_WIFI)
#include <WiFiUdp.h>
#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 <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)
const char* remoteIpAddress = nullptr;
unsigned short remotePort = 0;
//#if defined(HAS_WIFI)
char* broadcastIpAddress = nullptr;
WiFiUDP udp;
#endif
//#endif
void GetBroadcastAddress();
};

8
Arduino/ArduinoUtils.cpp
View File

@ -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

11
Arduino/Things/DRV8833.h
View File

@ -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 {

6
EspIdf/EspIdfParticipant.cpp
View File

@ -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();

2
EspIdf/EspIdfParticipant.h
View File

@ -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);

23
Participants/ParticipantUDP.cpp
View File

@ -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
<< "/" << (int)msg->networkId << "]\n";
std::cout << this->name << ": process PoseMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n";
#endif
}

2
Participants/ParticipantUDP.h
View File

@ -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);

33
Thing.cpp
View File

@ -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);

18
Thing.h
View File

@ -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;

12
Things/ServoMotor.cpp
View File

@ -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;
};
}
} // namespace RoboidControl

8
Things/ServoMotor.h
View File

@ -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