It compiles on Arduino Uno
This commit is contained in:
Pascal Serrarens
parent
ca35a59eb6
commit
f75bfe92d5
@ -19,8 +19,10 @@
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
|
||||
#if defined(ARDUINO)
|
||||
void LocalParticipant::Setup(int localPort,
|
||||
const char* remoteIpAddress,
|
||||
int remotePort) {
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
this->remoteIpAddress = remoteIpAddress;
|
||||
this->remotePort = remotePort;
|
||||
GetBroadcastAddress();
|
||||
@ -43,18 +45,19 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
|
||||
}
|
||||
|
||||
void LocalParticipant::GetBroadcastAddress() {
|
||||
#if defined(ARDUINO)
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
IPAddress broadcastAddress = WiFi.localIP();
|
||||
broadcastAddress[3] = 255;
|
||||
String broadcastIpString = broadcastAddress.toString();
|
||||
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";
|
||||
#endif
|
||||
}
|
||||
|
||||
void LocalParticipant::Receive() {
|
||||
#if defined(ARDUINO)
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
int packetSize = udp.parsePacket();
|
||||
while (packetSize > 0) {
|
||||
udp.read(buffer, packetSize);
|
||||
@ -64,12 +67,14 @@ void LocalParticipant::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);
|
||||
// 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
|
||||
// // std::cout << "New remote participant " <<
|
||||
// remoteParticipant->ipAddress
|
||||
// // << ":" << remoteParticipant->port << " "
|
||||
// // << (int)remoteParticipant->networkId << "\n";
|
||||
// }
|
||||
@ -82,7 +87,7 @@ void LocalParticipant::Receive() {
|
||||
}
|
||||
|
||||
bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
|
||||
#if defined(ARDUINO)
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
|
||||
// << remoteParticipant->port << "\n";
|
||||
|
||||
@ -102,7 +107,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
|
||||
}
|
||||
|
||||
bool LocalParticipant::Publish(IMessage* msg) {
|
||||
#ifdef ARDUINO
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
int bufferSize = msg->Serialize((char*)this->buffer);
|
||||
if (bufferSize <= 0)
|
||||
return true;
|
||||
|
||||
@ -2,6 +2,10 @@
|
||||
|
||||
#include "../LocalParticipant.h"
|
||||
|
||||
#if defined(HAS_WIFI)
|
||||
#include <WiFiUdp.h>
|
||||
#endif
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
@ -13,6 +17,14 @@ class LocalParticipant : public RoboidControl::LocalParticipant {
|
||||
bool Publish(IMessage* msg);
|
||||
|
||||
protected:
|
||||
#if defined(HAS_WIFI)
|
||||
const char* remoteIpAddress = nullptr;
|
||||
unsigned short remotePort = 0;
|
||||
char* broadcastIpAddress = nullptr;
|
||||
|
||||
WiFiUDP udp;
|
||||
#endif
|
||||
|
||||
void GetBroadcastAddress();
|
||||
};
|
||||
|
||||
|
||||
@ -42,8 +42,13 @@ struct NssServer {
|
||||
} nssServer;
|
||||
#endif
|
||||
|
||||
bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallback) {
|
||||
#if UNO_R4 || ARDUINO_ARCH_RP2040
|
||||
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");
|
||||
return false;
|
||||
@ -120,8 +125,10 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
|
||||
#if ESP32
|
||||
printf("Checking credentials in flash\n");
|
||||
wifiPreferences.begin(PREFERENCES_NAMESPACE);
|
||||
wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials));
|
||||
if (strcmp(wifiSsid, credentials.ssid) != 0 || strcmp(wifiPassword, credentials.password) != 0) {
|
||||
wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials,
|
||||
sizeof(credentials));
|
||||
if (strcmp(wifiSsid, credentials.ssid) != 0 ||
|
||||
strcmp(wifiPassword, credentials.password) != 0) {
|
||||
printf("Updating credentials in flash...");
|
||||
const int ssidLen = strlen(wifiSsid);
|
||||
if (ssidLen < 32) {
|
||||
@ -134,7 +141,8 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
|
||||
memcpy(credentials.password, wifiPassword, pwdLen);
|
||||
credentials.password[pwdLen] = '\0';
|
||||
}
|
||||
wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials));
|
||||
wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials,
|
||||
sizeof(credentials));
|
||||
printf(" completed.\n");
|
||||
}
|
||||
wifiPreferences.end();
|
||||
@ -142,10 +150,15 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallb
|
||||
}
|
||||
|
||||
return (!hotSpotEnabled);
|
||||
#endif
|
||||
}
|
||||
|
||||
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;
|
||||
#else
|
||||
Serial.println("Checking for firmware updates.");
|
||||
@ -177,10 +190,12 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
|
||||
switch (ret) {
|
||||
case HTTP_UPDATE_FAILED:
|
||||
#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());
|
||||
#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());
|
||||
#endif
|
||||
break;
|
||||
@ -198,5 +213,6 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
|
||||
Serial.println(httpCode);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
@ -29,7 +29,7 @@ else()
|
||||
)
|
||||
file(GLOB srcs
|
||||
*.cpp
|
||||
Sensors/*.cpp
|
||||
Things/*.cpp
|
||||
Messages/*.cpp
|
||||
Arduino/*.cpp
|
||||
Posix/*.cpp
|
||||
|
||||
@ -5,21 +5,23 @@
|
||||
#include "Arduino/ArduinoParticipant.h"
|
||||
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
#include "Windows/WindowsParticipant.h"
|
||||
#include <winsock2.h>
|
||||
#include <ws2tcpip.h>
|
||||
#include "Windows/WindowsParticipant.h"
|
||||
#pragma comment(lib, "ws2_32.lib")
|
||||
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
#include "Posix/PosixParticipant.h"
|
||||
#include <arpa/inet.h>
|
||||
#include <fcntl.h> // For fcntl
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
#include <chrono>
|
||||
#include "Posix/PosixParticipant.h"
|
||||
#endif
|
||||
|
||||
#include <string.h>
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
// LocalParticipant::LocalParticipant() {}
|
||||
@ -32,7 +34,8 @@ LocalParticipant::LocalParticipant(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;
|
||||
if (this->port == 0)
|
||||
this->isIsolated = true;
|
||||
@ -55,15 +58,20 @@ void LocalParticipant::begin() {
|
||||
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)
|
||||
Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
|
||||
Windows::LocalParticipant* thisWindows =
|
||||
static_cast<Windows::LocalParticipant*>(this);
|
||||
thisWindows->Setup(localPort, remoteIpAddress, remotePort);
|
||||
#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);
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
thisArduino->Setup(localPort, remoteIpAddress, remotePort);
|
||||
#endif
|
||||
this->connected = true;
|
||||
@ -71,13 +79,15 @@ void LocalParticipant::SetupUDP(int localPort, const char* remoteIpAddress, int
|
||||
|
||||
void LocalParticipant::Update(unsigned long currentTimeMs) {
|
||||
if (currentTimeMs == 0) {
|
||||
#if defined(ARDUINO)
|
||||
currentTimeMs = millis();
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
|
||||
currentTimeMs = static_cast<unsigned long>(ms.count());
|
||||
#endif
|
||||
currentTimeMs = Thing::GetTimeMs();
|
||||
// #if defined(ARDUINO)
|
||||
// currentTimeMs = millis();
|
||||
// #elif defined(__unix__) || defined(__APPLE__)
|
||||
// auto now = std::chrono::steady_clock::now();
|
||||
// auto ms =
|
||||
// std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
|
||||
// currentTimeMs = static_cast<unsigned long>(ms.count());
|
||||
// #endif
|
||||
}
|
||||
|
||||
if (this->isIsolated == false) {
|
||||
@ -112,13 +122,16 @@ void LocalParticipant::Update(unsigned long currentTimeMs) {
|
||||
|
||||
void LocalParticipant::ReceiveUDP() {
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
|
||||
Windows::LocalParticipant* thisWindows =
|
||||
static_cast<Windows::LocalParticipant*>(this);
|
||||
thisWindows->Receive();
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
|
||||
Posix::LocalParticipant* thisPosix =
|
||||
static_cast<Posix::LocalParticipant*>(this);
|
||||
thisPosix->Receive();
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
thisArduino->Receive();
|
||||
#endif
|
||||
}
|
||||
@ -132,17 +145,23 @@ Participant* LocalParticipant::GetParticipant(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);
|
||||
#if defined(NO_STD)
|
||||
participant->networkId = this->senderCount;
|
||||
this->senders[this->senderCount++] = participant;
|
||||
#else
|
||||
participant->networkId = (unsigned char)this->senders.size();
|
||||
this->senders.push_back(participant);
|
||||
#endif
|
||||
return participant;
|
||||
}
|
||||
|
||||
#pragma region Send
|
||||
|
||||
void LocalParticipant::SendThingInfo(Participant* remoteParticipant, Thing* thing) {
|
||||
std::cout << "Send thing info " << (int)thing->id << " \n";
|
||||
void LocalParticipant::SendThingInfo(Participant* remoteParticipant,
|
||||
Thing* thing) {
|
||||
// std::cout << "Send thing info " << (int)thing->id << " \n";
|
||||
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
||||
this->Send(remoteParticipant, thingMsg);
|
||||
delete thingMsg;
|
||||
@ -166,13 +185,16 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
|
||||
return true;
|
||||
|
||||
#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);
|
||||
#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);
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
return thisArduino->Send(remoteParticipant, bufferSize);
|
||||
#endif
|
||||
}
|
||||
@ -200,13 +222,16 @@ void LocalParticipant::PublishThingInfo(Thing* thing) {
|
||||
|
||||
bool LocalParticipant::Publish(IMessage* msg) {
|
||||
#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);
|
||||
#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);
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
return thisArduino->Publish(msg);
|
||||
#endif
|
||||
}
|
||||
@ -216,8 +241,11 @@ bool LocalParticipant::Publish(IMessage* msg) {
|
||||
|
||||
#pragma region Receive
|
||||
|
||||
void LocalParticipant::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) {
|
||||
Participant* remoteParticipant = this->GetParticipant(senderIpAddress, senderPort);
|
||||
void LocalParticipant::ReceiveData(unsigned char packetSize,
|
||||
char* senderIpAddress,
|
||||
unsigned int senderPort) {
|
||||
Participant* remoteParticipant =
|
||||
this->GetParticipant(senderIpAddress, senderPort);
|
||||
if (remoteParticipant == nullptr) {
|
||||
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
|
||||
// std::cout << "New sender " << sender_ipAddress << ":" << sender_port
|
||||
@ -230,7 +258,8 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, char* senderIpAddre
|
||||
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];
|
||||
// std::cout << "receive msg " << (int)msgId << "\n";
|
||||
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, 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) {
|
||||
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);
|
||||
}
|
||||
@ -295,22 +325,27 @@ void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
|
||||
int stringLen = nameLength + 1;
|
||||
char* thingName = new char[stringLen];
|
||||
#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
|
||||
// Use strncpy with bounds checking for other platforms (Arduino, POSIX, ESP-IDF)
|
||||
strncpy(thingName, msg->name, stringLen - 1); // Leave space for null terminator
|
||||
thingName[stringLen - 1] = '\0'; // Ensure null termination
|
||||
// Use strncpy with bounds checking for other platforms (Arduino, POSIX,
|
||||
// ESP-IDF)
|
||||
strncpy(thingName, msg->name,
|
||||
stringLen - 1); // Leave space for null terminator
|
||||
thingName[stringLen - 1] = '\0'; // Ensure null termination
|
||||
#endif
|
||||
thingName[nameLength] = '\0';
|
||||
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, BinaryMsg* msg) {
|
||||
// std::cout << this->name << ": process Binary [" << (int)this->networkId << "/"
|
||||
// std::cout << this->name << ": process Binary [" << (int)this->networkId <<
|
||||
// "/"
|
||||
// << (int)msg->networkId << "]\n";
|
||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
||||
if (thing != nullptr)
|
||||
@ -319,8 +354,9 @@ void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
|
||||
thing = this->Get(msg->networkId, msg->thingId);
|
||||
if (thing != nullptr)
|
||||
thing->ProcessBinary(msg->bytes);
|
||||
else
|
||||
std::cout << "custom msg for unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n";
|
||||
// else
|
||||
// std::cout << "custom msg for unknown thing [" << (int)msg->networkId
|
||||
// << "/" << (int)msg->thingId << "]\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -10,7 +10,9 @@
|
||||
#include "Messages/ThingMsg.h"
|
||||
#include "Participant.h"
|
||||
|
||||
#if !defined(NO_STD)
|
||||
#include <list>
|
||||
#endif
|
||||
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
#include <winsock2.h>
|
||||
@ -20,34 +22,41 @@
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
#elif defined(ARDUINO)
|
||||
#include <WiFiUdp.h>
|
||||
// #include <WiFiUdp.h>
|
||||
#endif
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief A local participant is the local device which can communicate with 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.
|
||||
constexpr int MAX_SENDER_COUNT = 256;
|
||||
|
||||
/// @brief A local participant is the local device which can communicate with
|
||||
/// 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
|
||||
/// constructor. In that case an hidden isolated participant is created which can be
|
||||
/// obtained using RoboidControl::LocalParticipant::Isolated().
|
||||
/// It is possible to work with an hidden participant by creating things without
|
||||
/// specifying a participant in the constructor. In that case an hidden isolated
|
||||
/// participant is created which can be obtained using
|
||||
/// RoboidControl::LocalParticipant::Isolated().
|
||||
/// @sa RoboidControl::Thing::Thing()
|
||||
class LocalParticipant : public Participant {
|
||||
public:
|
||||
/// @brief Create a participant without connecting to a site
|
||||
/// @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.
|
||||
/// Alternatively they can broadcast information which can be used directly by other participants.
|
||||
/// These participant typically broadcast Participant messages to let site
|
||||
/// 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);
|
||||
/// @brief Create a participant which will try to connect to a site.
|
||||
/// @param ipAddress The IP address of the site
|
||||
/// @param port The port used by the site
|
||||
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
|
||||
static LocalParticipant* Isolated();
|
||||
|
||||
@ -55,7 +64,8 @@ class LocalParticipant : public Participant {
|
||||
/// Isolated participants do not communicate with other participants
|
||||
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
|
||||
|
||||
/// @brief The name of the participant
|
||||
@ -67,11 +77,11 @@ class LocalParticipant : public Participant {
|
||||
Participant* remoteSite = nullptr;
|
||||
|
||||
#if defined(ARDUINO)
|
||||
const char* remoteIpAddress = nullptr;
|
||||
unsigned short remotePort = 0;
|
||||
char* broadcastIpAddress = nullptr;
|
||||
// const char* remoteIpAddress = nullptr;
|
||||
// unsigned short remotePort = 0;
|
||||
// char* broadcastIpAddress = nullptr;
|
||||
|
||||
WiFiUDP udp;
|
||||
// WiFiUDP udp;
|
||||
#else
|
||||
|
||||
#if defined(__unix__) || defined(__APPLE__)
|
||||
@ -94,10 +104,17 @@ class LocalParticipant : public Participant {
|
||||
bool Send(Participant* remoteParticipant, 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);
|
||||
|
||||
#if defined(NO_STD)
|
||||
unsigned char senderCount = 0;
|
||||
Participant* senders[MAX_SENDER_COUNT];
|
||||
#else
|
||||
std::list<Participant*> senders;
|
||||
#endif
|
||||
|
||||
protected:
|
||||
unsigned long nextPublishMe = 0;
|
||||
|
||||
@ -1,15 +1,18 @@
|
||||
#include "LowLevelMessages.h"
|
||||
|
||||
#include "LinearAlgebra/float16.h"
|
||||
#include <iostream>
|
||||
// #include <iostream>
|
||||
|
||||
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);
|
||||
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)++];
|
||||
|
||||
Angle8 angle = Angle8::Binary(binary);
|
||||
@ -17,14 +20,17 @@ Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer, unsigned char* startI
|
||||
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);
|
||||
short binary = value16.getBinary();
|
||||
|
||||
buffer[(*ix)++] = (binary >> 8) & 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 msb = buffer[ix++];
|
||||
unsigned char lsb = buffer[ix++];
|
||||
@ -36,12 +42,15 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer, unsigned char* startI
|
||||
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);
|
||||
SendAngle8(buffer, ix, s.direction.horizontal.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);
|
||||
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
@ -54,7 +63,9 @@ Spherical16 LowLevelMessages::ReceiveSpherical16(const char* buffer, unsigned ch
|
||||
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();
|
||||
unsigned char qx = (char)(q.x * 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;
|
||||
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)++] = qy;
|
||||
buffer[(*ix)++] = qz;
|
||||
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 qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
|
||||
@ -12,7 +12,8 @@ Participant::Participant(const char* ipAddress, int port) {
|
||||
int stringLength = addressLength + 1;
|
||||
char* addressString = new char[stringLength];
|
||||
#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
|
||||
strncpy(addressString, ipAddress, addressLength);
|
||||
#endif
|
||||
@ -32,7 +33,8 @@ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
|
||||
if (thing->id == thingId)
|
||||
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";
|
||||
return nullptr;
|
||||
}
|
||||
@ -40,34 +42,62 @@ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
|
||||
void Participant::Add(Thing* thing, bool checkId) {
|
||||
if (checkId && thing->id == 0) {
|
||||
// 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;
|
||||
this->things.push_back(thing);
|
||||
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" << this->port << "[" <<
|
||||
// (int)thing->networkId << "/"
|
||||
#endif
|
||||
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
|
||||
// this->port << "[" << (int)thing->networkId << "/"
|
||||
// << (int)thing->id << "]\n";
|
||||
} else {
|
||||
Thing* foundThing = Get(thing->networkId, thing->id);
|
||||
if (foundThing == nullptr) {
|
||||
#if defined(NO_STD)
|
||||
this->things[this->thingCount++] = thing;
|
||||
#else
|
||||
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";
|
||||
}
|
||||
// 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";
|
||||
}
|
||||
}
|
||||
|
||||
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; });
|
||||
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) {
|
||||
// // Not very efficient, but it works for now.
|
||||
|
||||
// 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
|
||||
// // << "\n";
|
||||
// thing->Update(currentTimeMs);
|
||||
|
||||
@ -3,16 +3,21 @@
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
constexpr int MAX_THING_COUNT = 256;
|
||||
|
||||
/// @brief A participant is a device which manages things.
|
||||
/// It can communicate with other participant to synchronise the state of things.
|
||||
/// This class is used to register the things the participant is managing.
|
||||
/// It also maintains the communcation information to contact the participant.
|
||||
/// It is used as a basis for the local participant, but also as a reference to remote participants.
|
||||
/// It can communicate with other participant to synchronise the state of
|
||||
/// things. This class is used to register the things the participant is
|
||||
/// managing. It also maintains the communcation information to contact the
|
||||
/// participant. It is used as a basis for the local participant, but also as a
|
||||
/// reference to remote participants.
|
||||
class Participant {
|
||||
public:
|
||||
/// @brief The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
|
||||
const char *ipAddress = "0.0.0.0";
|
||||
/// @brief The port number for UDP communication with the participant. This is 0 for isolated participants.
|
||||
public:
|
||||
/// @brief The Ip Address of a participant. When the participant is local,
|
||||
/// this contains 0.0.0.0
|
||||
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;
|
||||
|
||||
/// @brief The network Id to identify the participant.
|
||||
@ -24,29 +29,34 @@ public:
|
||||
/// @brief Create a new participant with the given communcation info
|
||||
/// @param ipAddress The IP address 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
|
||||
~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
|
||||
std::list<Thing *> things;
|
||||
std::list<Thing*> things;
|
||||
#endif
|
||||
|
||||
public:
|
||||
public:
|
||||
/// @brief Find a thing managed by this participant
|
||||
/// @param networkId The network ID for the thing
|
||||
/// @param thingId The ID of the thing
|
||||
/// @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.
|
||||
Thing *Get(unsigned char networkId, unsigned char thingId);
|
||||
/// @brief Add a new thing for this participant.
|
||||
Thing* Get(unsigned char networkId, unsigned char thingId);
|
||||
/// @brief Add a new thing for this participant.
|
||||
/// @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.
|
||||
void Add(Thing *thing, bool checkId = true);
|
||||
/// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
|
||||
/// Id will be assigned.
|
||||
void Add(Thing* thing, bool checkId = true);
|
||||
/// @brief Remove a thing for this participant
|
||||
/// @param thing The thing to remove
|
||||
void Remove(Thing *thing);
|
||||
|
||||
void Remove(Thing* thing);
|
||||
};
|
||||
|
||||
} // namespace Control
|
||||
} // namespace RoboidControl
|
||||
|
||||
@ -2,8 +2,10 @@
|
||||
|
||||
#include "Things/TemperatureSensor.h"
|
||||
|
||||
#if !defined(NO_STD)
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#endif
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
@ -14,37 +16,49 @@ SiteServer::SiteServer(int port) {
|
||||
this->ipAddress = "0.0.0.0";
|
||||
this->port = port;
|
||||
|
||||
#if defined(NO_STD)
|
||||
this->senders[this->senderCount++] = this;
|
||||
#else
|
||||
this->senders.push_back(this);
|
||||
#endif
|
||||
|
||||
SetupUDP(port, ipAddress, 0);
|
||||
|
||||
#if !defined(NO_STD)
|
||||
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) {
|
||||
std::cout << this->name << " received New Client -> " << sender->ipAddress
|
||||
<< ":" << (int)sender->port << "\n";
|
||||
SiteMsg *msg = new SiteMsg(sender->networkId);
|
||||
// std::cout << this->name << " received New Client -> " <<
|
||||
// sender->ipAddress
|
||||
// << ":" << (int)sender->port << "\n";
|
||||
SiteMsg* msg = new SiteMsg(sender->networkId);
|
||||
this->Send(sender, msg);
|
||||
delete msg;
|
||||
}
|
||||
}
|
||||
|
||||
void SiteServer::Process(Participant *sender, SiteMsg *msg) {}
|
||||
void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
|
||||
|
||||
void SiteServer::Process(Participant *sender, ThingMsg *msg) {
|
||||
Thing *thing = sender->Get(msg->networkId, msg->thingId);
|
||||
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
|
||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
||||
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);
|
||||
Thing *newThing;
|
||||
if (thingMsgProcessor != thingMsgProcessors.end()) // found item
|
||||
newThing = thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
|
||||
Thing* newThing;
|
||||
if (thingMsgProcessor != thingMsgProcessors.end()) // found item
|
||||
newThing =
|
||||
thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
|
||||
else
|
||||
newThing = new Thing(sender, msg->networkId, msg->thingId,
|
||||
(Thing::Type)msg->thingType);
|
||||
//sender->Add(newThing);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace Control
|
||||
} // namespace RoboidControl
|
||||
|
||||
14
SiteServer.h
14
SiteServer.h
@ -2,9 +2,11 @@
|
||||
|
||||
#include "LocalParticipant.h"
|
||||
|
||||
#if !defined(NO_STD)
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#endif
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
@ -15,12 +17,16 @@ class SiteServer : public LocalParticipant {
|
||||
|
||||
// virtual void Update(unsigned long currentTimeMs = 0) override;
|
||||
|
||||
#if !defined(NO_STD)
|
||||
template <typename ThingClass>
|
||||
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);
|
||||
};
|
||||
};
|
||||
#endif
|
||||
|
||||
protected:
|
||||
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, 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;
|
||||
#endif
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
|
||||
40
Thing.cpp
40
Thing.cpp
@ -1,15 +1,18 @@
|
||||
#include "Thing.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <list>
|
||||
#include <chrono>
|
||||
|
||||
#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() {
|
||||
// if (isolatedParticipant == nullptr)
|
||||
@ -17,10 +20,10 @@ namespace RoboidControl {
|
||||
// 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) {
|
||||
this->owner = owner;
|
||||
@ -38,7 +41,10 @@ Thing::Thing(Participant* owner, int thingType) {
|
||||
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..
|
||||
this->owner = owner;
|
||||
this->networkId = networkId;
|
||||
@ -47,7 +53,8 @@ Thing::Thing(Participant* owner, unsigned char networkId, unsigned char thingId,
|
||||
|
||||
this->linearVelocity = 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";
|
||||
owner->Add(this, false);
|
||||
}
|
||||
@ -166,18 +173,18 @@ void Thing::SetModel(const char* url) {
|
||||
}
|
||||
|
||||
unsigned long Thing::GetTimeMs() {
|
||||
#if defined(ARDUINO)
|
||||
return millis();
|
||||
#else
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
now.time_since_epoch());
|
||||
return static_cast<unsigned long>(ms.count());
|
||||
#endif
|
||||
}
|
||||
|
||||
void Thing::Update() {
|
||||
#if defined(ARDUINO)
|
||||
Update(millis());
|
||||
#else
|
||||
Update(GetTimeMs());
|
||||
#endif
|
||||
}
|
||||
|
||||
void Thing::Update(unsigned long currentTimeMs) {
|
||||
@ -186,7 +193,6 @@ void Thing::Update(unsigned long currentTimeMs) {
|
||||
// PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
|
||||
// participant->Send(remoteParticipant, poseMsg);
|
||||
// delete poseMsg;
|
||||
|
||||
}
|
||||
|
||||
void Thing::UpdateThings(unsigned long currentTimeMs) {
|
||||
|
||||
19
Thing.h
19
Thing.h
@ -1,6 +1,9 @@
|
||||
#pragma once
|
||||
|
||||
#if !defined(NO_STD)
|
||||
#include <iostream>
|
||||
#include <list>
|
||||
#endif
|
||||
#include "LinearAlgebra/Spherical.h"
|
||||
#include "LinearAlgebra/SwingTwist.h"
|
||||
|
||||
@ -49,7 +52,10 @@ class Thing {
|
||||
/// @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);
|
||||
Thing(Participant* participant,
|
||||
unsigned char networkId,
|
||||
unsigned char thingId,
|
||||
Type thingType = Type::Undetermined);
|
||||
|
||||
/// @brief The participant managing this thing
|
||||
Participant* owner;
|
||||
@ -105,7 +111,8 @@ class Thing {
|
||||
public:
|
||||
/// @brief The name of the thing
|
||||
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;
|
||||
/// @brief The scale of the model (deprecated I think)
|
||||
float modelScale = 1;
|
||||
@ -123,7 +130,7 @@ class Thing {
|
||||
/// @return The orienation in local space
|
||||
SwingTwist16 GetOrientation();
|
||||
/// @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
|
||||
bool positionUpdated = false;
|
||||
@ -131,7 +138,8 @@ class Thing {
|
||||
bool orientationUpdated = false;
|
||||
|
||||
/// @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);
|
||||
/// @brief Get the linear velocity of the thing
|
||||
/// @return The linear velocity in local space, in meters per second
|
||||
@ -177,7 +185,8 @@ class Thing {
|
||||
|
||||
/// @brief Updates the state of the thing
|
||||
/// @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);
|
||||
|
||||
|
||||
@ -2,14 +2,18 @@
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {
|
||||
// this->leftWheel = new Thing(participant);
|
||||
// this->rightWheel = new Thing(participant);
|
||||
RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
|
||||
: Thing(participant) {
|
||||
this->leftWheel = new Thing(participant);
|
||||
this->rightWheel = new Thing(participant);
|
||||
}
|
||||
|
||||
void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) {
|
||||
this->wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
|
||||
this->wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
|
||||
void DifferentialDrive::SetDimensions(float wheelDiameter,
|
||||
float wheelSeparation) {
|
||||
this->wheelRadius =
|
||||
wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
|
||||
this->wheelSeparation =
|
||||
wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
|
||||
this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
|
||||
|
||||
float distance = this->wheelSeparation / 2;
|
||||
@ -22,6 +26,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation
|
||||
void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
|
||||
float distance = this->wheelSeparation / 2;
|
||||
this->leftWheel = leftWheel;
|
||||
;
|
||||
if (leftWheel != nullptr)
|
||||
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
|
||||
|
||||
@ -41,16 +46,22 @@ void DifferentialDrive::Update(unsigned long currentMs) {
|
||||
angularSpeed = -angularSpeed;
|
||||
|
||||
// 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)
|
||||
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)
|
||||
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 "
|
||||
// << speedLeft << " right wheel " << speedRight << "\n";
|
||||
// std::cout << "lin. speed " << linearVelocity << " ang. speed " <<
|
||||
// angularVelocity.distance << " left wheel "
|
||||
// << speedLeft << " right wheel " << speedRight << "\n";
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -6,9 +6,11 @@ namespace RoboidControl {
|
||||
|
||||
// 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) {}
|
||||
|
||||
void TemperatureSensor::SetTemperature(float temp) {
|
||||
@ -16,7 +18,7 @@ void TemperatureSensor::SetTemperature(float temp) {
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
@ -10,8 +10,8 @@ TouchSensor::TouchSensor(Participant* participant) : Thing(participant) {
|
||||
void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
|
||||
|
||||
void TouchSensor::ProcessBinary(char* bytes) {
|
||||
if (bytes[0] == 1)
|
||||
std::cout << this->name << " is Touching something!\n";
|
||||
// if (bytes[0] == 1)
|
||||
// std::cout << this->name << " is Touching something!\n";
|
||||
this->touchedSomething |= (bytes[0] == 1);
|
||||
}
|
||||
|
||||
|
||||
20
library.json
Normal file
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"
|
||||
]
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user