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

It compiles on Arduino Uno

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-07 11:47:45 +01:00
parent ca35a59eb6
commit f75bfe92d5
17 changed files with 377 additions and 166 deletions

27
Arduino/ArduinoParticipant.cpp

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

12
Arduino/ArduinoParticipant.h

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

32
Arduino/ArduinoUtils.cpp

@ -42,8 +42,13 @@ struct NssServer {
} nssServer; } nssServer;
#endif #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) { if (WiFi.status() == WL_NO_MODULE) {
Serial.println("WiFi not present, WiFiSync is disabled"); Serial.println("WiFi not present, WiFiSync is disabled");
return false; return false;
@ -120,8 +125,10 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
#if ESP32 #if ESP32
printf("Checking credentials in flash\n"); printf("Checking credentials in flash\n");
wifiPreferences.begin(PREFERENCES_NAMESPACE); 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..."); printf("Updating credentials in flash...");
const int ssidLen = strlen(wifiSsid); const int ssidLen = strlen(wifiSsid);
if (ssidLen < 32) { if (ssidLen < 32) {
@ -134,7 +141,8 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
memcpy(credentials.password, wifiPassword, pwdLen); memcpy(credentials.password, wifiPassword, pwdLen);
credentials.password[pwdLen] = '\0'; credentials.password[pwdLen] = '\0';
} }
wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials)); wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials,
sizeof(credentials));
printf(" completed.\n"); printf(" completed.\n");
} }
wifiPreferences.end(); wifiPreferences.end();
@ -142,10 +150,15 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
} }
return (!hotSpotEnabled); return (!hotSpotEnabled);
#endif
} }
void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) { 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; return;
#else #else
Serial.println("Checking for firmware updates."); Serial.println("Checking for firmware updates.");
@ -177,10 +190,12 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
switch (ret) { switch (ret) {
case HTTP_UPDATE_FAILED: case HTTP_UPDATE_FAILED:
#if defined(ESP32) #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()); httpUpdate.getLastErrorString().c_str());
#else #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()); ESPhttpUpdate.getLastErrorString().c_str());
#endif #endif
break; break;
@ -198,5 +213,6 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
Serial.println(httpCode); Serial.println(httpCode);
} }
#endif #endif
#endif
} }
#endif #endif

2
CMakeLists.txt

@ -29,7 +29,7 @@ else()
) )
file(GLOB srcs file(GLOB srcs
*.cpp *.cpp
Sensors/*.cpp Things/*.cpp
Messages/*.cpp Messages/*.cpp
Arduino/*.cpp Arduino/*.cpp
Posix/*.cpp Posix/*.cpp

114
LocalParticipant.cpp

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

53
LocalParticipant.h

@ -10,7 +10,9 @@
#include "Messages/ThingMsg.h" #include "Messages/ThingMsg.h"
#include "Participant.h" #include "Participant.h"
#if !defined(NO_STD)
#include <list> #include <list>
#endif
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h> #include <winsock2.h>
@ -20,34 +22,41 @@
#include <sys/socket.h> #include <sys/socket.h>
#include <unistd.h> #include <unistd.h>
#elif defined(ARDUINO) #elif defined(ARDUINO)
#include <WiFiUdp.h> // #include <WiFiUdp.h>
#endif #endif
namespace RoboidControl { 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() /// @sa RoboidControl::Thing::Thing()
class LocalParticipant : public Participant { class LocalParticipant : public Participant {
public: public:
/// @brief Create a participant without connecting to a site /// @brief Create a participant without connecting to a site
/// @param port The port on which the participant communicates /// @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); LocalParticipant(int port = 7681);
/// @brief Create a participant which will try to connect to a site. /// @brief Create a participant which will try to connect to a site.
/// @param ipAddress The IP address of the site /// @param ipAddress The IP address of the site
/// @param port The port used by the site /// @param port The port used by the site
LocalParticipant(const char* ipAddress, int port = 7681); 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 /// @return A participant without networking support
static LocalParticipant* Isolated(); static LocalParticipant* Isolated();
@ -55,7 +64,8 @@ class LocalParticipant : public Participant {
/// Isolated participants do not communicate with other participants /// Isolated participants do not communicate with other participants
bool isIsolated = false; 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 long publishInterval = 3000; // 3 seconds
/// @brief The name of the participant /// @brief The name of the participant
@ -67,11 +77,11 @@ class LocalParticipant : public Participant {
Participant* remoteSite = nullptr; Participant* remoteSite = nullptr;
#if defined(ARDUINO) #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 #else
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
@ -94,10 +104,17 @@ class LocalParticipant : public Participant {
bool Send(Participant* remoteParticipant, IMessage* msg); bool Send(Participant* remoteParticipant, IMessage* msg);
bool Publish(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); 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; std::list<Participant*> senders;
#endif
protected: protected:
unsigned long nextPublishMe = 0; unsigned long nextPublishMe = 0;

33
Messages/LowLevelMessages.cpp

@ -1,15 +1,18 @@
#include "LowLevelMessages.h" #include "LowLevelMessages.h"
#include "LinearAlgebra/float16.h" #include "LinearAlgebra/float16.h"
#include <iostream> // #include <iostream>
namespace RoboidControl { namespace RoboidControl {
void LowLevelMessages::SendAngle8(char* buffer, unsigned char* ix, const float angle) { void LowLevelMessages::SendAngle8(char* buffer,
unsigned char* ix,
const float angle) {
Angle8 packedAngle2 = Angle8::Degrees(angle); Angle8 packedAngle2 = Angle8::Degrees(angle);
buffer[(*ix)++] = packedAngle2.GetBinary(); buffer[(*ix)++] = packedAngle2.GetBinary();
} }
Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer, unsigned char* startIndex) { Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
unsigned char* startIndex) {
unsigned char binary = buffer[(*startIndex)++]; unsigned char binary = buffer[(*startIndex)++];
Angle8 angle = Angle8::Binary(binary); Angle8 angle = Angle8::Binary(binary);
@ -17,14 +20,17 @@ Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer, unsigned char* startI
return angle; return angle;
} }
void LowLevelMessages::SendFloat16(char* buffer, unsigned char* ix, float value) { void LowLevelMessages::SendFloat16(char* buffer,
unsigned char* ix,
float value) {
float16 value16 = float16(value); float16 value16 = float16(value);
short binary = value16.getBinary(); short binary = value16.getBinary();
buffer[(*ix)++] = (binary >> 8) & 0xFF; buffer[(*ix)++] = (binary >> 8) & 0xFF;
buffer[(*ix)++] = binary & 0xFF; buffer[(*ix)++] = binary & 0xFF;
} }
float LowLevelMessages::ReceiveFloat16(const char* buffer, unsigned char* startIndex) { float LowLevelMessages::ReceiveFloat16(const char* buffer,
unsigned char* startIndex) {
unsigned char ix = *startIndex; unsigned char ix = *startIndex;
unsigned char msb = buffer[ix++]; unsigned char msb = buffer[ix++];
unsigned char lsb = buffer[ix++]; unsigned char lsb = buffer[ix++];
@ -36,12 +42,15 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer, unsigned char* startI
return (float)f.toFloat(); return (float)f.toFloat();
} }
void LowLevelMessages::SendSpherical16(char* buffer, unsigned char* ix, Spherical16 s) { void LowLevelMessages::SendSpherical16(char* buffer,
unsigned char* ix,
Spherical16 s) {
SendFloat16(buffer, ix, s.distance); SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees()); SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees()); SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
} }
Spherical16 LowLevelMessages::ReceiveSpherical16(const char* buffer, unsigned char* startIndex) { Spherical16 LowLevelMessages::ReceiveSpherical16(const char* buffer,
unsigned char* startIndex) {
float distance = ReceiveFloat16(buffer, startIndex); float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex); Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
@ -54,7 +63,9 @@ Spherical16 LowLevelMessages::ReceiveSpherical16(const char* buffer, unsigned ch
return s; return s;
} }
void LowLevelMessages::SendQuat32(char* buffer, unsigned char* ix, SwingTwist16 rotation) { void LowLevelMessages::SendQuat32(char* buffer,
unsigned char* ix,
SwingTwist16 rotation) {
Quaternion q = rotation.ToQuaternion(); Quaternion q = rotation.ToQuaternion();
unsigned char qx = (char)(q.x * 127 + 128); unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128); unsigned char qy = (char)(q.y * 127 + 128);
@ -66,14 +77,16 @@ void LowLevelMessages::SendQuat32(char* buffer, unsigned char* ix, SwingTwist16
qz = -qz; qz = -qz;
qw = -qw; qw = -qw;
} }
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw << "\n"; // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
// << "\n";
buffer[(*ix)++] = qx; buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy; buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz; buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw; buffer[(*ix)++] = qw;
} }
SwingTwist16 LowLevelMessages::ReceiveQuat32(const char* buffer, unsigned char* ix) { SwingTwist16 LowLevelMessages::ReceiveQuat32(const char* buffer,
unsigned char* ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F; float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F; float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F; float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;

46
Participant.cpp

@ -12,7 +12,8 @@ Participant::Participant(const char* ipAddress, int port) {
int stringLength = addressLength + 1; int stringLength = addressLength + 1;
char* addressString = new char[stringLength]; char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress, addressLength); // Leave space for null terminator strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else #else
strncpy(addressString, ipAddress, addressLength); strncpy(addressString, ipAddress, addressLength);
#endif #endif
@ -32,7 +33,8 @@ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
if (thing->id == thingId) if (thing->id == thingId)
return thing; return thing;
} }
// std::cout << "Could not find thing " << this->ipAddress << ":" << this->port // std::cout << "Could not find thing " << this->ipAddress << ":" <<
// this->port
// << "[" << (int)networkId << "/" << (int)thingId << "]\n"; // << "[" << (int)networkId << "/" << (int)thingId << "]\n";
return nullptr; return nullptr;
} }
@ -40,34 +42,62 @@ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
void Participant::Add(Thing* thing, bool checkId) { void Participant::Add(Thing* thing, bool checkId) {
if (checkId && thing->id == 0) { if (checkId && thing->id == 0) {
// allocate a new thing ID // 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; thing->id = (unsigned char)this->things.size() + 1;
this->things.push_back(thing); this->things.push_back(thing);
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" << this->port << "[" << #endif
// (int)thing->networkId << "/" // std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
// this->port << "[" << (int)thing->networkId << "/"
// << (int)thing->id << "]\n"; // << (int)thing->id << "]\n";
} else { } else {
Thing* foundThing = Get(thing->networkId, thing->id); Thing* foundThing = Get(thing->networkId, thing->id);
if (foundThing == nullptr) { if (foundThing == nullptr) {
#if defined(NO_STD)
this->things[this->thingCount++] = thing;
#else
this->things.push_back(thing); this->things.push_back(thing);
// std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" << (int)thing->networkId << "/" #endif
// std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
// "[" << (int)thing->networkId << "/"
// << (int)thing->id << "]\n"; // << (int)thing->id << "]\n";
} }
// else // else
// std::cout << "Did not add, existing thing " << this->ipAddress << ":" << this->port << "[" // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
// << this->port << "["
// << (int)thing->networkId << "/" << (int)thing->id << "]\n"; // << (int)thing->networkId << "/" << (int)thing->id << "]\n";
} }
} }
void Participant::Remove(Thing* thing) { 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; }); this->things.remove_if([thing](Thing* obj) { return obj == thing; });
std::cout << "Removing " << thing->networkId << "/" << thing->id << " list size = " << this->things.size() << "\n"; std::cout << "Removing " << thing->networkId << "/" << thing->id
<< " list size = " << this->things.size() << "\n";
#endif
} }
// void Participant::UpdateAll(unsigned long currentTimeMs) { // void Participant::UpdateAll(unsigned long currentTimeMs) {
// // Not very efficient, but it works for now. // // Not very efficient, but it works for now.
// for (Thing* thing : this->things) { // for (Thing* thing : this->things) {
// if (thing != nullptr && thing->GetParent() == nullptr) { // update all root things // if (thing != nullptr && thing->GetParent() == nullptr) { // update all
// root things
// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id // // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// // << "\n"; // // << "\n";
// thing->Update(currentTimeMs); // thing->Update(currentTimeMs);

48
Participant.h

@ -3,16 +3,21 @@
namespace RoboidControl { namespace RoboidControl {
constexpr int MAX_THING_COUNT = 256;
/// @brief A participant is a device which manages things. /// @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 { class Participant {
public: 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,
const char *ipAddress = "0.0.0.0"; /// this contains 0.0.0.0
/// @brief The port number for UDP communication with the participant. This is 0 for isolated participants. const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant. This is
/// 0 for isolated participants.
int port = 0; int port = 0;
/// @brief The network Id to identify the participant. /// @brief The network Id to identify the participant.
@ -24,29 +29,34 @@ public:
/// @brief Create a new participant with the given communcation info /// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant /// @param ipAddress The IP address of the participant
/// @param port The port of the participant /// @param port The port of the participant
Participant(const char *ipAddress, int port); Participant(const char* ipAddress, int port);
/// @brief Destructor for the participant /// @brief Destructor for the participant
~Participant(); ~Participant();
protected: protected:
#if defined(NO_STD)
unsigned char thingCount = 0;
Thing* things[MAX_THING_COUNT];
#else
/// @brief The list of things managed by this participant /// @brief The list of things managed by this participant
std::list<Thing *> things; std::list<Thing*> things;
#endif
public: public:
/// @brief Find a thing managed by this participant /// @brief Find a thing managed by this participant
/// @param networkId The network ID for the thing /// @param networkId The network ID for the thing
/// @param thingId The ID of the thing /// @param thingId The ID of the thing
/// @return The thing if found or nullptr when no thing has been found /// @return The thing if found or nullptr when no thing has been found
/// @note The use of the network ID is likely to disappear in future versions. /// @note The use of the network ID is likely to disappear in future versions.
Thing *Get(unsigned char networkId, unsigned char thingId); Thing* Get(unsigned char networkId, unsigned char thingId);
/// @brief Add a new thing for this participant. /// @brief Add a new thing for this participant.
/// @param thing The thing to add /// @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
void Add(Thing *thing, bool checkId = true); /// Id will be assigned.
void Add(Thing* thing, bool checkId = true);
/// @brief Remove a thing for this participant /// @brief Remove a thing for this participant
/// @param thing The thing to remove /// @param thing The thing to remove
void Remove(Thing *thing); void Remove(Thing* thing);
}; };
} // namespace Control } // namespace RoboidControl

38
SiteServer.cpp

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

14
SiteServer.h

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

40
Thing.cpp

@ -1,15 +1,18 @@
#include "Thing.h" #include "Thing.h"
#include <string.h>
#include <algorithm>
#include <iostream>
#include <list>
#include <chrono>
#include "LocalParticipant.h" #include "LocalParticipant.h"
namespace RoboidControl { #include <string.h>
// #include <algorithm>
// #include <iostream>
// #include <list>
// #include <chrono>
#if defined(ARDUINO)
#include "Arduino.h"
#endif
namespace RoboidControl {
// LocalParticipant* Thing::CheckHiddenParticipant() { // LocalParticipant* Thing::CheckHiddenParticipant() {
// if (isolatedParticipant == nullptr) // if (isolatedParticipant == nullptr)
@ -17,10 +20,10 @@ namespace RoboidControl {
// return isolatedParticipant; // return isolatedParticipant;
// } // }
Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) { Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) {}
}
Thing::Thing(Participant* owner, Type thingType) : Thing(owner, (unsigned char)thingType) {} Thing::Thing(Participant* owner, Type thingType)
: Thing(owner, (unsigned char)thingType) {}
Thing::Thing(Participant* owner, int thingType) { Thing::Thing(Participant* owner, int thingType) {
this->owner = owner; this->owner = owner;
@ -38,7 +41,10 @@ Thing::Thing(Participant* owner, int thingType) {
owner->Add(this); owner->Add(this);
} }
Thing::Thing(Participant* owner, unsigned char networkId, unsigned char thingId, Type thingType) { Thing::Thing(Participant* owner,
unsigned char networkId,
unsigned char thingId,
Type thingType) {
// no participant reference yet.. // no participant reference yet..
this->owner = owner; this->owner = owner;
this->networkId = networkId; this->networkId = networkId;
@ -47,7 +53,8 @@ Thing::Thing(Participant* owner, unsigned char networkId, unsigned char thingId,
this->linearVelocity = Spherical16::zero; this->linearVelocity = Spherical16::zero;
this->angularVelocity = Spherical16::zero; this->angularVelocity = Spherical16::zero;
// std::cout << "Created thing " << (int)this->networkId << "/" << (int)this->id // std::cout << "Created thing " << (int)this->networkId << "/" <<
// (int)this->id
// << "\n"; // << "\n";
owner->Add(this, false); owner->Add(this, false);
} }
@ -166,18 +173,18 @@ void Thing::SetModel(const char* url) {
} }
unsigned long Thing::GetTimeMs() { unsigned long Thing::GetTimeMs() {
#if defined(ARDUINO)
return millis();
#else
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>( auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now.time_since_epoch()); now.time_since_epoch());
return static_cast<unsigned long>(ms.count()); return static_cast<unsigned long>(ms.count());
#endif
} }
void Thing::Update() { void Thing::Update() {
#if defined(ARDUINO)
Update(millis());
#else
Update(GetTimeMs()); Update(GetTimeMs());
#endif
} }
void Thing::Update(unsigned long currentTimeMs) { void Thing::Update(unsigned long currentTimeMs) {
@ -186,7 +193,6 @@ void Thing::Update(unsigned long currentTimeMs) {
// PoseMsg* poseMsg = new PoseMsg(this->networkId, this); // PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
// participant->Send(remoteParticipant, poseMsg); // participant->Send(remoteParticipant, poseMsg);
// delete poseMsg; // delete poseMsg;
} }
void Thing::UpdateThings(unsigned long currentTimeMs) { void Thing::UpdateThings(unsigned long currentTimeMs) {

19
Thing.h

@ -1,6 +1,9 @@
#pragma once #pragma once
#if !defined(NO_STD)
#include <iostream> #include <iostream>
#include <list> #include <list>
#endif
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h" #include "LinearAlgebra/SwingTwist.h"
@ -49,7 +52,10 @@ class Thing {
/// @param networkId The network ID of the thing /// @param networkId The network ID of the thing
/// @param thingId The ID of the thing /// @param thingId The ID of the thing
/// @param thingType The type of thing /// @param thingType The type of thing
Thing(Participant* participant, unsigned char networkId, unsigned char thingId, Type thingType = Type::Undetermined); Thing(Participant* participant,
unsigned char networkId,
unsigned char thingId,
Type thingType = Type::Undetermined);
/// @brief The participant managing this thing /// @brief The participant managing this thing
Participant* owner; Participant* owner;
@ -105,7 +111,8 @@ class Thing {
public: public:
/// @brief The name of the thing /// @brief The name of the thing
const char* name = nullptr; const char* name = nullptr;
/// @brief An URL pointing to the location where a model of the thing can be found /// @brief An URL pointing to the location where a model of the thing can be
/// found
const char* modelUrl = nullptr; const char* modelUrl = nullptr;
/// @brief The scale of the model (deprecated I think) /// @brief The scale of the model (deprecated I think)
float modelScale = 1; float modelScale = 1;
@ -123,7 +130,7 @@ class Thing {
/// @return The orienation in local space /// @return The orienation in local space
SwingTwist16 GetOrientation(); SwingTwist16 GetOrientation();
/// @brief The scale of the thing (deprecated I think) /// @brief The scale of the thing (deprecated I think)
//float scale = 1; // assuming uniform scale // float scale = 1; // assuming uniform scale
/// @brief boolean indicating if the position was updated /// @brief boolean indicating if the position was updated
bool positionUpdated = false; bool positionUpdated = false;
@ -131,7 +138,8 @@ class Thing {
bool orientationUpdated = false; bool orientationUpdated = false;
/// @brief Set the linear velocity of the thing /// @brief Set the linear velocity of the thing
/// @param linearVelocity The new linear velocity in local space, in meters per second /// @param linearVelocity The new linear velocity in local space, in meters
/// per second
void SetLinearVelocity(Spherical16 linearVelocity); void SetLinearVelocity(Spherical16 linearVelocity);
/// @brief Get the linear velocity of the thing /// @brief Get the linear velocity of the thing
/// @return The linear velocity in local space, in meters per second /// @return The linear velocity in local space, in meters per second
@ -177,7 +185,8 @@ class Thing {
/// @brief Updates the state of the thing /// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds /// @param currentTimeMs The current clock time in milliseconds
virtual void Update(unsigned long currentTimeMs); // { (void)currentTimeMs; }; virtual void Update(
unsigned long currentTimeMs); // { (void)currentTimeMs; };
static void UpdateThings(unsigned long currentTimeMs); static void UpdateThings(unsigned long currentTimeMs);

33
Things/DifferentialDrive.cpp

@ -2,14 +2,18 @@
namespace RoboidControl { namespace RoboidControl {
RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) { RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
// this->leftWheel = new Thing(participant); : Thing(participant) {
// this->rightWheel = new Thing(participant); this->leftWheel = new Thing(participant);
this->rightWheel = new Thing(participant);
} }
void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) { void DifferentialDrive::SetDimensions(float wheelDiameter,
this->wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2; float wheelSeparation) {
this->wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; this->wheelRadius =
wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
this->wheelSeparation =
wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi; this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
float distance = this->wheelSeparation / 2; float distance = this->wheelSeparation / 2;
@ -22,6 +26,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation
void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) { void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
float distance = this->wheelSeparation / 2; float distance = this->wheelSeparation / 2;
this->leftWheel = leftWheel; this->leftWheel = leftWheel;
;
if (leftWheel != nullptr) if (leftWheel != nullptr)
this->leftWheel->SetPosition(Spherical(distance, Direction::left)); this->leftWheel->SetPosition(Spherical(distance, Direction::left));
@ -41,16 +46,22 @@ void DifferentialDrive::Update(unsigned long currentMs) {
angularSpeed = -angularSpeed; angularSpeed = -angularSpeed;
// wheel separation can be replaced by this->leftwheel->position->distance // wheel separation can be replaced by this->leftwheel->position->distance
float speedLeft = (linearVelocity + angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg; float speedLeft =
(linearVelocity + angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
if (this->leftWheel != nullptr) if (this->leftWheel != nullptr)
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left)); this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
float speedRight = (linearVelocity - angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg; float speedRight =
(linearVelocity - angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
if (this->rightWheel != nullptr) if (this->rightWheel != nullptr)
this->rightWheel->SetAngularVelocity(Spherical(speedRight, Direction::right)); this->rightWheel->SetAngularVelocity(
Spherical(speedRight, Direction::right));
// std::cout << "lin. speed " << linearVelocity << " ang. speed " << angularVelocity.distance << " left wheel " // std::cout << "lin. speed " << linearVelocity << " ang. speed " <<
// << speedLeft << " right wheel " << speedRight << "\n"; // angularVelocity.distance << " left wheel "
// << speedLeft << " right wheel " << speedRight << "\n";
} }
} // namespace RoboidControl } // namespace RoboidControl

8
Things/TemperatureSensor.cpp

@ -6,9 +6,11 @@ namespace RoboidControl {
// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {} // TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
//TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {} // 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) {} : Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
void TemperatureSensor::SetTemperature(float temp) { void TemperatureSensor::SetTemperature(float temp) {
@ -16,7 +18,7 @@ void TemperatureSensor::SetTemperature(float temp) {
} }
void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) { 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); LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
} }

4
Things/TouchSensor.cpp

@ -10,8 +10,8 @@ TouchSensor::TouchSensor(Participant* participant) : Thing(participant) {
void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {} void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
void TouchSensor::ProcessBinary(char* bytes) { 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); this->touchedSomething |= (bytes[0] == 1);
} }

20
library.json Normal file

@ -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"
]
}
}