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

AR mode works, but reaction time is slow

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-21 15:59:11 +02:00
parent 1b5fef15e6
commit 140138977a
29 changed files with 313 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Arduino/ArduinoParticipant.cpp
View File

@ -82,19 +82,6 @@ void ParticipantUDP::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->Get(sender_ipAddress,
// sender_port); if (remoteParticipant == nullptr) {
// remoteParticipant = this->Add(sender_ipAddress,
// sender_port);
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
// // << "\n";
// // std::cout << "New remote participant " <<
// remoteParticipant->ipAddress
// // << ":" << remoteParticipant->port << " "
// // << (int)remoteParticipant->networkId << "\n";
// }
// ReceiveData(packetSize, remoteParticipant);
ReceiveData(packetSize, sender_ipAddress, sender_port); ReceiveData(packetSize, sender_ipAddress, sender_port);
packetSize = udp->parsePacket(); packetSize = udp->parsePacket();
} }
@ -107,6 +94,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
// << remoteParticipant->port << "\n"; // << remoteParticipant->port << "\n";
int n = 0; int n = 0;
int r = 0;
do { do {
if (n > 0) { if (n > 0) {
#if !defined(NO_STD) #if !defined(NO_STD)
@ -115,9 +103,13 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
delay(10); delay(10);
} }
n++; n++;
udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
udp->write((unsigned char*)buffer, bufferSize); udp->write((unsigned char*)buffer, bufferSize);
} while (udp->endPacket() == 0 && n < 10); r = udp->endPacket();
// On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
// It is not cleary yet why
} while (r == 0 && n < 10);
#endif #endif
return true; return true;

18
Arduino/Things/DRV8833.cpp
View File

@ -7,14 +7,14 @@ namespace Arduino {
#pragma region DRV8833 #pragma region DRV8833
DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) { DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
this->pinStandby = config.standby; this->pinStandby = config.standby;
if (pinStandby != 255) if (pinStandby != 255)
pinMode(pinStandby, OUTPUT); pinMode(pinStandby, OUTPUT);
this->motorA = new DRV8833Motor(*this, config.AIn1, config.AIn2); this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
this->motorA->SetName("Motor A"); this->motorA->SetName("Motor A");
this->motorB = new DRV8833Motor(*this, config.BIn1, config.BIn2); this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
this->motorB->SetName("Motor B"); this->motorB->SetName("Motor B");
} }
@ -23,11 +23,11 @@ DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) {
#pragma region Differential drive #pragma region Differential drive
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
Thing& parent) Thing* parent)
: RoboidControl::DifferentialDrive(parent), drv8833(config) { : RoboidControl::DifferentialDrive(this->drv8833.motorA,
this->leftWheel = this->drv8833.motorA; this->drv8833.motorB,
this->rightWheel = this->drv8833.motorB; parent),
} drv8833(config, this) {}
void DRV8833::DifferentialDrive::Update(bool recurse) { void DRV8833::DifferentialDrive::Update(bool recurse) {
RoboidControl::DifferentialDrive::Update(recurse); RoboidControl::DifferentialDrive::Update(recurse);
@ -42,7 +42,7 @@ void DRV8833::DifferentialDrive::Update(bool recurse) {
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
#endif #endif
DRV8833Motor::DRV8833Motor(DRV8833& driver, DRV8833Motor::DRV8833Motor(DRV8833* driver,
unsigned char pinIn1, unsigned char pinIn1,
unsigned char pinIn2, unsigned char pinIn2,
bool reverse) bool reverse)

11
Arduino/Things/DRV8833.h
View File

@ -18,12 +18,11 @@ class DRV8833 : public Thing {
int AIn2; int AIn2;
int BIn1; int BIn1;
int BIn2; int BIn2;
int standby = 255; int standby;
}; };
/// @brief Setup a DRV8833 motor controller /// @brief Setup a DRV8833 motor controller
// DRV8833(Configuration config, Participant* owner = nullptr); DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
DRV8833(Configuration config, Thing& parent = Thing::LocalRoot());
DRV8833Motor* motorA = nullptr; DRV8833Motor* motorA = nullptr;
DRV8833Motor* motorB = nullptr; DRV8833Motor* motorB = nullptr;
@ -39,9 +38,7 @@ class DRV8833 : public Thing {
class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
public: public:
// DifferentialDrive(DRV8833::Configuration config, DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
// Participant* participant = nullptr);
DifferentialDrive(DRV8833::Configuration config, Thing& parent = Thing::LocalRoot());
virtual void Update(bool recurse = false) override; virtual void Update(bool recurse = false) override;
@ -60,7 +57,7 @@ class DRV8833Motor : public Motor {
/// @param pinIn1 the pin number for the in1 signal /// @param pinIn1 the pin number for the in1 signal
/// @param pinIn2 the pin number for the in2 signal /// @param pinIn2 the pin number for the in2 signal
/// @param direction the forward turning direction of the motor /// @param direction the forward turning direction of the motor
DRV8833Motor(DRV8833& driver, DRV8833Motor(DRV8833* driver,
unsigned char pinIn1, unsigned char pinIn1,
unsigned char pinIn2, unsigned char pinIn2,
bool reverse = false); bool reverse = false);

6
Arduino/Things/DigitalInput.cpp
View File

@ -7,7 +7,7 @@ namespace Arduino {
#pragma region Digital input #pragma region Digital input
DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) { DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) {
this->pin = pin; this->pin = pin;
pinMode(this->pin, INPUT); pinMode(this->pin, INPUT);
std::cout << "digital input start\n"; std::cout << "digital input start\n";
@ -23,7 +23,7 @@ void DigitalInput::Update(bool recursive) {
#pragma region Touch sensor #pragma region Touch sensor
DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent) DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
: RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {} : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
void DigitalInput::TouchSensor::Update(bool recursive) { void DigitalInput::TouchSensor::Update(bool recursive) {
@ -39,7 +39,7 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0; volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
Thing& parent) Thing* parent)
: RoboidControl::RelativeEncoder(parent), : RoboidControl::RelativeEncoder(parent),
digitalInput(config.pin, parent), digitalInput(config.pin, parent),
pulsesPerRevolution(config.pulsesPerRevolution) {} pulsesPerRevolution(config.pulsesPerRevolution) {}

8
Arduino/Things/DigitalInput.h
View File

@ -12,9 +12,9 @@ class DigitalInput : public Thing {
/// @brief Create a new digital input /// @brief Create a new digital input
/// @param participant The participant to use /// @param participant The participant to use
/// @param pin The digital pin /// @param pin The digital pin
DigitalInput(Participant* participant, unsigned char pin); //DigitalInput(Participant* participant, unsigned char pin);
// DigitalInput(Thing* parent, unsigned char pin); // DigitalInput(Thing* parent, unsigned char pin);
DigitalInput(unsigned char pin, Thing& parent); DigitalInput(unsigned char pin, Thing* parent);
bool isHigh = false; bool isHigh = false;
bool isLow = false; bool isLow = false;
@ -35,7 +35,7 @@ class DigitalInput : public Thing {
class DigitalInput::TouchSensor : public RoboidControl::TouchSensor { class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
public: public:
TouchSensor(unsigned char pin, Thing& parent); TouchSensor(unsigned char pin, Thing* parent);
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recurse = false) override; virtual void Update(bool recurse = false) override;
@ -55,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
unsigned char pulsesPerRevolution; unsigned char pulsesPerRevolution;
}; };
RelativeEncoder(Configuration config, Thing& parent = Thing::LocalRoot()); RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
unsigned char pulsesPerRevolution; unsigned char pulsesPerRevolution;

17
Arduino/Things/UltrasonicSensor.cpp
View File

@ -6,8 +6,9 @@
namespace RoboidControl { namespace RoboidControl {
namespace Arduino { namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent) UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: Thing(parent) { : Thing(Type::Undetermined, parent) {
this->name = "Ultrasonic sensor";
this->pinTrigger = config.triggerPin; this->pinTrigger = config.triggerPin;
this->pinEcho = config.echoPin; this->pinEcho = config.echoPin;
@ -57,16 +58,14 @@ void UltrasonicSensor::Update(bool recursive) {
#pragma region Touch sensor #pragma region Touch sensor
UltrasonicSensor::TouchSensor::TouchSensor( UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
UltrasonicSensor::Configuration config, : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
Thing& parent)
: RoboidControl::TouchSensor(parent), ultrasonic(config) {}
void UltrasonicSensor::TouchSensor::Update(bool recursive) { void UltrasonicSensor::TouchSensor::Update(bool recursive) {
this->ultrasonic.Update(false);
this->touchedSomething = (this->ultrasonic.distance > 0 &&
this->ultrasonic.distance <= this->touchDistance);
RoboidControl::TouchSensor::Update(recursive); RoboidControl::TouchSensor::Update(recursive);
this->ultrasonic.Update(false);
this->touchedSomething |= (this->ultrasonic.distance > 0 &&
this->ultrasonic.distance <= this->touchDistance);
} }
#pragma region Touch sensor #pragma region Touch sensor

4
Arduino/Things/UltrasonicSensor.h
View File

@ -13,7 +13,7 @@ class UltrasonicSensor : Thing {
int echoPin; int echoPin;
}; };
UltrasonicSensor(Configuration config, Thing& parent = Thing::LocalRoot()); UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
// parameters // parameters
@ -47,7 +47,7 @@ class UltrasonicSensor : Thing {
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
public: public:
TouchSensor(UltrasonicSensor::Configuration config, TouchSensor(UltrasonicSensor::Configuration config,
Thing& parent = Thing::LocalRoot()); Thing* parent = Thing::LocalRoot());
float touchDistance = 0.2f; float touchDistance = 0.2f;

12
Messages/ThingMsg.cpp
View File

@ -17,19 +17,11 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
if (thing->IsRoot()) if (thing->IsRoot())
this->parentId = 0; this->parentId = 0;
else { else {
Thing parent = thing->GetParent(); Thing* parent = thing->GetParent();
this->parentId = parent.id; this->parentId = parent->id;
} }
} }
// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
// unsigned char thingType, unsigned char parentId) {
// this->networkId = networkId;
// this->thingId = thingId;
// this->thingType = thingType;
// this->parentId = parentId;
// }
ThingMsg::~ThingMsg() {} ThingMsg::~ThingMsg() {}
unsigned char ThingMsg::Serialize(char* buffer) { unsigned char ThingMsg::Serialize(char* buffer) {

59
Participant.cpp
View File

@ -8,14 +8,26 @@ namespace RoboidControl {
ParticipantRegistry Participant::registry; ParticipantRegistry Participant::registry;
//Participant Participant::LocalParticipant = Participant("0.0.0.0", 0); Participant* Participant::LocalParticipant = new Participant();
Participant& Participant::LocalParticipant() { void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
static Participant localParticipant("0.0.0.0", 0); LocalParticipant = &newParticipant;
return localParticipant; std::cout << "Replaced local participant" << std::endl;
} }
Participant::Participant() {
std::cout << "P\n";
//this->root.name = "Isolated";
this->root = new Thing(this);
this->root->name = "Root";
this->Add(this->root);
}
Participant::Participant(const char* ipAddress, int port) { Participant::Participant(const char* ipAddress, int port) {
// Add the root thing to the list of things, because we could not do that
// earlier (I think)
this->Add(this->root);
// make a copy of the ip address string // make a copy of the ip address string
int addressLength = (int)strlen(ipAddress); int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1; int stringLength = addressLength + 1;
@ -33,7 +45,7 @@ Participant::Participant(const char* ipAddress, int port) {
} }
Participant::~Participant() { Participant::~Participant() {
registry.Remove(this); // registry.Remove(this);
delete[] this->ipAddress; delete[] this->ipAddress;
} }
@ -56,9 +68,7 @@ Thing* Participant::Get(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) #if defined(NO_STD)
thing->id = this->thingCount + 1; thing->id = this->thingCount + 1;
@ -76,7 +86,7 @@ void Participant::Add(Thing* thing, bool checkId) {
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
// << this->port << "[" << (int)thing->id << "]\n"; // << this->port << "[" << (int)thing->id << "]\n";
} else { } else {
Thing* foundThing = Get(thing->id); Thing* foundThing = Get(thing->id);
if (foundThing == nullptr) { if (foundThing == nullptr) {
@ -128,8 +138,8 @@ Participant* ParticipantRegistry::Get(const char* ipAddress,
continue; continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 && if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) { participant->port == port) {
std::cout << "found participant " << participant->ipAddress << ":" // std::cout << "found participant " << participant->ipAddress << ":"
<< (int)participant->port << std::endl; // << (int)participant->port << std::endl;
return participant; return participant;
} }
} }
@ -140,7 +150,7 @@ Participant* ParticipantRegistry::Get(const char* ipAddress,
} }
Participant* ParticipantRegistry::Get(unsigned char participantId) { Participant* ParticipantRegistry::Get(unsigned char participantId) {
#if !defined(NO_STD) #if !defined(NO_STD)
for (Participant* participant : ParticipantRegistry::participants) { for (Participant* participant : ParticipantRegistry::participants) {
if (participant == nullptr) if (participant == nullptr)
continue; continue;
@ -148,7 +158,7 @@ Participant* ParticipantRegistry::Get(unsigned char participantId) {
return participant; return participant;
} }
std::cout << "Could not find participant " << (int)participantId << std::endl; std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif #endif
return nullptr; return nullptr;
} }
@ -165,19 +175,22 @@ void ParticipantRegistry::Add(Participant* participant) {
if (foundParticipant == nullptr) { if (foundParticipant == nullptr) {
#if defined(NO_STD) #if defined(NO_STD)
//this->things[this->thingCount++] = thing; // this->things[this->thingCount++] = thing;
#else #else
ParticipantRegistry::participants.push_back(participant); ParticipantRegistry::participants.push_back(participant);
#endif #endif
// std::cout << "Add participant " << participant->ipAddress << ":" // std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId // << participant->port << "[" << (int)participant->networkId
// << "]\n"; // << "]\n";
// std::cout << "participants " << ParticipantRegistry::participants.size() // std::cout << "participants " <<
// << "\n"; // ParticipantRegistry::participants.size()
// } else { // << "\n";
// std::cout << "Did not add, existing participant " << participant->ipAddress // } else {
// << ":" << participant->port << "[" << (int)participant->networkId // std::cout << "Did not add, existing participant " <<
// << "]\n"; // participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
} }
} }

12
Participant.h
View File

@ -1,4 +1,5 @@
#pragma once #pragma once
#include "Thing.h" #include "Thing.h"
namespace RoboidControl { namespace RoboidControl {
@ -36,6 +37,7 @@ class ParticipantRegistry {
public: public:
Participant** GetAll() const; Participant** GetAll() const;
int count = 0; int count = 0;
private: private:
Participant** participants; Participant** participants;
#else #else
@ -43,6 +45,7 @@ class ParticipantRegistry {
/// @brief Get all participants /// @brief Get all participants
/// @return All participants /// @return All participants
const std::list<Participant*>& GetAll() const; const std::list<Participant*>& GetAll() const;
private: private:
/// @brief The list of known participants /// @brief The list of known participants
std::list<Participant*> participants; std::list<Participant*> participants;
@ -57,6 +60,9 @@ class ParticipantRegistry {
/// reference to remote participants. /// reference to remote participants.
class Participant { class Participant {
public: public:
/// @brief The name of the participant
const char* name = "Participant";
/// @brief The Ip Address of a participant. /// @brief The Ip Address of a participant.
const char* ipAddress = "0.0.0.0"; const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant. /// @brief The port number for UDP communication with the participant.
@ -65,6 +71,7 @@ class Participant {
/// @brief The network Id to identify the participant /// @brief The network Id to identify the participant
unsigned char networkId = 0; unsigned char networkId = 0;
Participant();
/// @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 UDP port of the participant /// @param port The UDP port of the participant
@ -72,7 +79,10 @@ class Participant {
/// @brief Destructor for the participant /// @brief Destructor for the participant
~Participant(); ~Participant();
static Participant& LocalParticipant(); static Participant* LocalParticipant;
static void ReplaceLocalParticipant(Participant& newParticipant);
Thing* root = new Thing(this);
public: public:
#if defined(NO_STD) #if defined(NO_STD)

50
Participants/ParticipantUDP.cpp
View File

@ -1,5 +1,6 @@
#include "ParticipantUDP.h" #include "ParticipantUDP.h"
#include "Participant.h"
#include "Thing.h" #include "Thing.h"
#include "Arduino/ArduinoParticipant.h" #include "Arduino/ArduinoParticipant.h"
@ -11,20 +12,38 @@
namespace RoboidControl { namespace RoboidControl {
#pragma region Init
ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
this->name = "ParticipantUDP";
this->remoteSite = nullptr; this->remoteSite = nullptr;
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
Participant::registry.Add(this); Participant::registry.Add(this);
this->root = Thing::LocalRoot(); //::LocalParticipant->root;
this->root->owner = this;
this->root->name = "UDP Root";
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
} }
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: Participant("127.0.0.1", localPort) { : Participant("127.0.0.1", localPort) {
this->name = "ParticipantUDP";
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
else else
this->remoteSite = new Participant(ipAddress, port); this->remoteSite = new Participant(ipAddress, port);
Participant::registry.Add(this); Participant::registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
this->root->owner = this;
this->root->name = "UDP Root";
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
} }
static ParticipantUDP* isolatedParticipant = nullptr; static ParticipantUDP* isolatedParticipant = nullptr;
@ -64,11 +83,19 @@ void ParticipantUDP::SetupUDP(int localPort,
this->connected = true; this->connected = true;
} }
#pragma endregion Init
#pragma region Update #pragma region Update
// The update order
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDP::Update() { void ParticipantUDP::Update() {
unsigned long currentTimeMs = Thing::GetTimeMs(); unsigned long currentTimeMs = Thing::GetTimeMs();
PrepMyThings();
if (this->isIsolated == false) { if (this->isIsolated == false) {
if (this->connected == false) if (this->connected == false)
begin(); begin();
@ -91,11 +118,22 @@ void ParticipantUDP::Update() {
UpdateOtherThings(); UpdateOtherThings();
} }
void ParticipantUDP::PrepMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->PrepareForUpdate();
}
}
void ParticipantUDP::UpdateMyThings() { void ParticipantUDP::UpdateMyThings() {
// std::cout << this->things.size() << std::endl;
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing == nullptr) // || thing->GetParent() != nullptr) if (thing == nullptr) // || thing->GetParent() != nullptr)
continue; continue;
// std::cout << thing->name << "\n";
if (thing->hierarchyChanged) { if (thing->hierarchyChanged) {
if (!(this->isIsolated || this->networkId == 0)) { if (!(this->isIsolated || this->networkId == 0)) {
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
@ -110,12 +148,14 @@ void ParticipantUDP::UpdateMyThings() {
} }
} }
// std::cout << "B\n";
// Why don't we do recursive? // Why don't we do recursive?
// Because when a thing creates a thing in the update, // Because when a thing creates a thing in the update,
// that new thing is not sent out (because of hierarchyChanged) // that new thing is not sent out (because of hierarchyChanged)
// before it is updated itself: it is immediatedly updated! // before it is updated itself: it is immediatedly updated!
thing->Update(false); thing->Update(false);
// std::cout << "C\n";
if (!(this->isIsolated || this->networkId == 0)) { if (!(this->isIsolated || this->networkId == 0)) {
if (thing->terminate) { if (thing->terminate) {
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
@ -136,8 +176,10 @@ void ParticipantUDP::UpdateMyThings() {
delete binaryMsg; delete binaryMsg;
} }
} }
// std::cout << "D\n";
if (thing->terminate) if (thing->terminate)
this->Remove(thing); this->Remove(thing);
// std::cout << "E\n";
} }
} }
@ -200,8 +242,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
if (bufferSize <= 0) if (bufferSize <= 0)
return true; return true;
// std::cout << "send msg " << (int)this->buffer[0] << " to " // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << remoteParticipant->ipAddress << std::endl; // << " to " << remoteParticipant->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows = Windows::ParticipantUDP* thisWindows =
@ -378,14 +420,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process SiteMsg " << (int)this->networkId std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
<< " -> " << (int)msg->networkId << "\n"; << " -> " << (int)msg->networkId << "\n";
#endif #endif
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);
} }

5
Participants/ParticipantUDP.h
View File

@ -78,9 +78,6 @@ class ParticipantUDP : public Participant {
/// local network /// local network
long publishInterval = 3000; // 3 seconds long publishInterval = 3000; // 3 seconds
/// @brief The name of the participant
const char* name = "ParticipantUDP";
protected: protected:
char buffer[1024]; char buffer[1024];
@ -105,6 +102,8 @@ class ParticipantUDP : public Participant {
protected: protected:
unsigned long nextPublishMe = 0; unsigned long nextPublishMe = 0;
/// @brief Prepare the local things for the next update
virtual void PrepMyThings();
virtual void UpdateMyThings(); virtual void UpdateMyThings();
virtual void UpdateOtherThings(); virtual void UpdateOtherThings();

7
Participants/SiteServer.cpp
View File

@ -73,10 +73,13 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->thingId); Thing* thing = sender->Get(msg->thingId);
if (thing == nullptr) if (thing == nullptr)
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
Thing::Reconstruct(sender, msg->thingType, msg->thingId); // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
//thing = new Thing(msg->thingType, sender->root);
;
thing->id = msg->thingId;
if (msg->parentId != 0) { if (msg->parentId != 0) {
thing->SetParent(*Get(msg->parentId)); thing->SetParent(Get(msg->parentId));
if (thing->IsRoot()) if (thing->IsRoot())
// if (thing->GetParent() != nullptr) // if (thing->GetParent() != nullptr)
#if defined(NO_STD) #if defined(NO_STD)

88
Thing.cpp
View File

@ -20,12 +20,31 @@ namespace RoboidControl {
#pragma region Init #pragma region Init
Thing& Thing::LocalRoot() { Thing* Thing::LocalRoot() {
static Thing localRoot(Thing::Type::Undetermined, localRoot); Participant* p = Participant::LocalParticipant;
Thing* localRoot = p->root;
return localRoot; return localRoot;
} }
Thing::Thing(unsigned char thingType, Thing& parent) { // Only use this for root things
Thing::Thing(Participant* owner) {
this->type = Type::Roboid; // should become root
this->position = Spherical::zero;
this->positionUpdated = true;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->hierarchyChanged = true;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
this->owner = owner;
//this->owner->Add(this, true);
std::cout << this->owner->name << ": New root thing " << std::endl;
}
Thing::Thing(unsigned char thingType, Thing* parent) {
this->type = thingType; this->type = thingType;
this->position = Spherical::zero; this->position = Spherical::zero;
@ -37,9 +56,16 @@ Thing::Thing(unsigned char thingType, Thing& parent) {
this->linearVelocity = Spherical::zero; this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero; this->angularVelocity = Spherical::zero;
this->owner = &Participant::LocalParticipant(); this->owner = parent->owner;
this->owner->Add(this, true); this->owner->Add(this, true);
this->SetParent(parent); this->SetParent(parent);
std::cout << this->owner->name << ": New thing for " << parent->name
<< std::endl;
}
Thing::~Thing() {
std::cout << "Destroy thing " << this->name << std::endl;
} }
// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, // Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
@ -66,28 +92,28 @@ void Thing::SetModel(const char* url) {
#pragma region Hierarchy #pragma region Hierarchy
// void Thing::SetParent(Thing* parent) { void Thing::SetParent(Thing* parent) {
// if (parent == nullptr) { if (parent == nullptr) {
// Thing* parentThing = this->parent; Thing* parentThing = this->parent;
// if (parentThing != nullptr) if (parentThing != nullptr)
// parentThing->RemoveChild(this); parentThing->RemoveChild(this);
// this->parent = nullptr; this->parent = nullptr;
// } else } else
// parent->AddChild(this); parent->AddChild(this);
// this->hierarchyChanged = true;
// }
void Thing::SetParent(Thing& parent) {
parent.AddChild(this);
this->hierarchyChanged = true; this->hierarchyChanged = true;
} }
const Thing& Thing::GetParent() { // void Thing::SetParent(Thing* parent) {
return *this->parent; // parent->AddChild(this);
} // this->hierarchyChanged = true;
// }
// const Thing& Thing::GetParent() {
// return *this->parent;
// }
bool Thing::IsRoot() const { bool Thing::IsRoot() const {
return this == &LocalRoot(); //&Thing::Root; return this == LocalRoot() || this->parent == nullptr; //&Thing::Root;
} }
// void Thing::SetParent(Thing* root, const char* name) { // void Thing::SetParent(Thing* root, const char* name) {
@ -96,9 +122,9 @@ bool Thing::IsRoot() const {
// this->SetParent(thing); // this->SetParent(thing);
// } // }
// Thing* Thing::GetParent() { Thing* Thing::GetParent() {
// return this->parent; return this->parent;
// } }
Thing* Thing::GetChildByIndex(unsigned char ix) { Thing* Thing::GetChildByIndex(unsigned char ix) {
return this->children[ix]; return this->children[ix];
@ -110,11 +136,11 @@ void Thing::AddChild(Thing* child) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
newChildren[childIx] = this->children[childIx]; newChildren[childIx] = this->children[childIx];
// if (this->children[childIx] == child) { if (this->children[childIx] == child) {
// // child is already present, stop copying do not update the children // child is already present, stop copying do not update the children
// delete[] newChildren; delete[] newChildren;
// return; return;
// } }
} }
newChildren[this->childCount] = child; newChildren[this->childCount] = child;
@ -143,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) {
} }
} }
child->parent = &Thing::LocalRoot(); child->parent = Thing::LocalRoot();
delete[] this->children; delete[] this->children;
this->children = newChildren; this->children = newChildren;
@ -248,6 +274,8 @@ unsigned long Thing::GetTimeMs() {
// Update(GetTimeMs(), recursive); // Update(GetTimeMs(), recursive);
// } // }
void Thing::PrepareForUpdate() {}
void Thing::Update(bool recursive) { void Thing::Update(bool recursive) {
// if (this->positionUpdated || this->orientationUpdated) // if (this->positionUpdated || this->orientationUpdated)
// OnPoseChanged callback // OnPoseChanged callback

29
Thing.h
View File

@ -41,8 +41,15 @@ class Thing {
}; };
#pragma region Init #pragma region Init
static Thing& LocalRoot(); static Thing* LocalRoot();
private:
// Special constructor to create a root thing
Thing(Participant* parent);
// Which can only be used by the Participant
friend class Participant;
public:
/// @brief Create a new thing /// @brief Create a new thing
/// @param thingType The type of thing (can use Thing::Type) /// @param thingType The type of thing (can use Thing::Type)
/// @param parent (optional) The parent thing /// @param parent (optional) The parent thing
@ -50,7 +57,7 @@ class Thing {
/// be Participant::LocalParticipant if the parent is not specified. A thing /// be Participant::LocalParticipant if the parent is not specified. A thing
/// without a parent will be a root thing. /// without a parent will be a root thing.
Thing(unsigned char thingType = Thing::Type::Undetermined, Thing(unsigned char thingType = Thing::Type::Undetermined,
Thing& parent = LocalRoot()); Thing* parent = LocalRoot());
/// @brief Create a new child thing /// @brief Create a new child thing
/// @param parent The parent thing /// @param parent The parent thing
@ -59,6 +66,8 @@ class Thing {
/// an ID /// an ID
/// @note The owner will be the same as the owner of the parent thing /// @note The owner will be the same as the owner of the parent thing
~Thing();
static Thing Reconstruct(Participant* owner, static Thing Reconstruct(Participant* owner,
unsigned char thingType, unsigned char thingType,
unsigned char thingId); unsigned char thingId);
@ -73,6 +82,7 @@ class Thing {
/// @brief The participant managing this thing /// @brief The participant managing this thing
Participant* owner = nullptr; Participant* owner = nullptr;
/// @brief The ID of the thing /// @brief The ID of the thing
unsigned char id = 0; unsigned char id = 0;
@ -108,11 +118,11 @@ class Thing {
/// @brief Sets the parent of this Thing /// @brief Sets the parent of this Thing
/// @param parent The Thing which should become the parent /// @param parent The Thing which should become the parent
// virtual void SetParent(Thing* parent); // virtual void SetParent(Thing* parent);
void SetParent(Thing& parent); void SetParent(Thing* parent);
/// @brief Gets the parent of this Thing /// @brief Gets the parent of this Thing
/// @return The parent Thing /// @return The parent Thing
// Thing* GetParent(); // Thing* GetParent();
const Thing& GetParent(); Thing* GetParent();
bool IsRoot() const; bool IsRoot() const;
@ -215,12 +225,7 @@ class Thing {
#pragma region Update #pragma region Update
public: public:
/// @brief Get the current time in milliseconds virtual void PrepareForUpdate();
/// @return The current time in milliseconds
static unsigned long GetTimeMs();
/// @brief Updates the state of the thing
// void Update(bool recursive = false);
/// @brief Updates the state of the thing /// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds; if this is /// @param currentTimeMs The current clock time in milliseconds; if this is
@ -230,6 +235,10 @@ class Thing {
static void UpdateThings(); static void UpdateThings();
/// @brief Get the current time in milliseconds
/// @return The current time in milliseconds
static unsigned long GetTimeMs();
#pragma endregion Update #pragma endregion Update
public: public:

14
Things/ControlledMotor.cpp
View File

@ -2,9 +2,9 @@
namespace RoboidControl { namespace RoboidControl {
ControlledMotor::ControlledMotor(Motor& motor, ControlledMotor::ControlledMotor(Motor* motor,
RelativeEncoder& encoder, RelativeEncoder* encoder,
Thing& parent) Thing* parent)
: Motor(parent), motor(motor), encoder(encoder) { : Motor(parent), motor(motor), encoder(encoder) {
this->type = Type::ControlledMotor; this->type = Type::ControlledMotor;
// encoder.SetParent(*this); // encoder.SetParent(*this);
@ -19,9 +19,9 @@ void ControlledMotor::SetTargetVelocity(float velocity) {
} }
void ControlledMotor::Update(bool recurse) { void ControlledMotor::Update(bool recurse) {
encoder.Update(false); encoder->Update(false);
this->actualVelocity = (int)rotationDirection * encoder.rotationSpeed; this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
unsigned long currentTimeMs = GetTimeMs(); unsigned long currentTimeMs = GetTimeMs();
float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
@ -31,10 +31,10 @@ void ControlledMotor::Update(bool recurse) {
float acceleration = float acceleration =
error * timeStep * pidP; // Just P is used at this moment error * timeStep * pidP; // Just P is used at this moment
std::cout << "motor acc. " << acceleration << std::endl; std::cout << "motor acc. " << acceleration << std::endl;
motor.SetTargetVelocity(targetVelocity + motor->SetTargetVelocity(targetVelocity +
acceleration); // or something like that acceleration); // or something like that
motor.Update(false); motor->Update(false);
} }
// float ControlledMotor::GetActualVelocity() { // float ControlledMotor::GetActualVelocity() {

6
Things/ControlledMotor.h
View File

@ -10,7 +10,7 @@ namespace RoboidControl {
/// The speed is measured in revolutions per second. /// The speed is measured in revolutions per second.
class ControlledMotor : public Motor { class ControlledMotor : public Motor {
public: public:
ControlledMotor(Motor& motor, RelativeEncoder& encoder, Thing& parent = Thing::LocalRoot()); ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
float pidP = 1; float pidP = 1;
float pidD = 0; float pidD = 0;
@ -28,8 +28,8 @@ class ControlledMotor : public Motor {
/// @param speed the target velocity in revolutions per second /// @param speed the target velocity in revolutions per second
virtual void SetTargetVelocity(float velocity) override; virtual void SetTargetVelocity(float velocity) override;
Motor& motor; Motor* motor;
RelativeEncoder& encoder; RelativeEncoder* encoder;
protected: protected:
float lastUpdateTime; float lastUpdateTime;

41
Things/DifferentialDrive.cpp
View File

@ -1,11 +1,27 @@
#include "DifferentialDrive.h" #include "DifferentialDrive.h"
#include "Messages/LowLevelMessages.h"
namespace RoboidControl { namespace RoboidControl {
DifferentialDrive::DifferentialDrive(Thing& parent) DifferentialDrive::DifferentialDrive(Thing* parent)
: Thing(Type::DifferentialDrive, parent) { : Thing(Type::DifferentialDrive, parent) {
this->leftWheel = new Motor(); this->name = "Differential drive";
this->rightWheel = new Motor();
this->leftWheel = new Motor(this);
this->leftWheel->name = "Left motor";
this->rightWheel = new Motor(this);
this->rightWheel->name = "Right motor";
}
DifferentialDrive::DifferentialDrive(Motor* leftMotor,
Motor* rightMotor,
Thing* parent)
: Thing(Type::DifferentialDrive, parent) {
this->name = "Differential drive";
this->leftWheel = leftMotor;
this->rightWheel = rightMotor;
} }
void DifferentialDrive::SetDriveDimensions(float wheelDiameter, void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
@ -23,13 +39,13 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
this->rightWheel->SetPosition(Spherical(distance, Direction::right)); this->rightWheel->SetPosition(Spherical(distance, Direction::right));
} }
Motor& DifferentialDrive::GetMotorLeft() { // Motor& DifferentialDrive::GetMotorLeft() {
return *this->leftWheel; // return *this->leftWheel;
} // }
Motor& DifferentialDrive::GetMotorRight() { // Motor& DifferentialDrive::GetMotorRight() {
return *this->rightWheel; // return *this->rightWheel;
} // }
void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) { void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
float distance = this->wheelSeparation / 2; float distance = this->wheelSeparation / 2;
@ -78,4 +94,11 @@ void DifferentialDrive::Update(bool recursive) {
Thing::Update(recursive); Thing::Update(recursive);
} }
int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->leftWheel->id;
data[(*ix)++] = this->rightWheel->id;
LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
return 4;
}
} // namespace RoboidControl } // namespace RoboidControl

23
Things/DifferentialDrive.h
View File

@ -14,7 +14,9 @@ class DifferentialDrive : public Thing {
/// @param parent The parent thing /// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate /// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID /// an ID
DifferentialDrive(Thing& parent = Thing::LocalRoot()); DifferentialDrive(Thing* parent = Thing::LocalRoot());
DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot());
/// @brief Configures the dimensions of the drive /// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters /// @param wheelDiameter The diameter of the wheels in meters
@ -25,8 +27,8 @@ class DifferentialDrive : public Thing {
/// @sa SetLinearVelocity SetAngularVelocity /// @sa SetLinearVelocity SetAngularVelocity
void SetDriveDimensions(float wheelDiameter, float wheelSeparation); void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
Motor& GetMotorLeft(); // Motor& GetMotorLeft();
Motor& GetMotorRight(); // Motor& GetMotorRight();
/// @brief Congures the motors for the wheels /// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left wheel /// @param leftWheel The motor for the left wheel
/// @param rightWheel The motor for the right wheel /// @param rightWheel The motor for the right wheel
@ -42,20 +44,23 @@ class DifferentialDrive : public Thing {
/// @copydoc RoboidControl::Thing::Update(unsigned long) /// @copydoc RoboidControl::Thing::Update(unsigned long)
virtual void Update(bool recursive = true) override; virtual void Update(bool recursive = true) override;
int GenerateBinary(char* bytes, unsigned char* ix) override;
// virtual void ProcessBinary(char* bytes) override;
/// @brief The left wheel
Motor* leftWheel = nullptr;
/// @brief The right wheel
Motor* rightWheel = nullptr;
protected: protected:
/// @brief The radius of a wheel in meters /// @brief The radius of a wheel in meters
float wheelRadius = 1.0f; float wheelRadius = 0.0f;
/// @brief The distance between the wheels in meters /// @brief The distance between the wheels in meters
float wheelSeparation = 1.0f; float wheelSeparation = 0.0f;
/// @brief Convert revolutions per second to meters per second /// @brief Convert revolutions per second to meters per second
float rpsToMs = 1.0f; float rpsToMs = 1.0f;
/// @brief The left wheel
Motor* leftWheel = nullptr;
/// @brief The right wheel
Motor* rightWheel = nullptr;
}; };
} // namespace RoboidControl } // namespace RoboidControl

2
Things/DigitalSensor.cpp
View File

@ -13,7 +13,7 @@ namespace RoboidControl {
// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} // DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {}
// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} // DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {}
DigitalSensor::DigitalSensor(Thing& parent) : Thing(Type::Switch, parent) {} DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {}
int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
bytes[(*ix)++] = state ? 1 : 0; bytes[(*ix)++] = state ? 1 : 0;

2
Things/DigitalSensor.h
View File

@ -19,7 +19,7 @@ class DigitalSensor : public Thing {
/// @param thingId The ID of the thing, leave out or set to zero to generate /// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID /// an ID
// DigitalSensor(Thing* parent); //, unsigned char thingId = 0); // DigitalSensor(Thing* parent); //, unsigned char thingId = 0);
DigitalSensor(Thing& parent = Thing::LocalRoot()); DigitalSensor(Thing* parent = Thing::LocalRoot());
/// @brief The sigital state /// @brief The sigital state
bool state = 0; bool state = 0;

15
Things/Motor.cpp
View File

@ -2,16 +2,15 @@
namespace RoboidControl { namespace RoboidControl {
// RoboidControl::Motor::Motor(Participant* owner) Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
// : Thing(owner, Type::UncontrolledMotor) {}
// RoboidControl::Motor::Motor(Thing* parent) void Motor::SetTargetVelocity(float targetSpeed) {
// : Thing(parent, Type::UncontrolledMotor) {}
Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {}
void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) {
this->targetVelocity = targetSpeed; this->targetVelocity = targetSpeed;
} }
int Motor::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->targetVelocity * 127.0f;
return 1;
}
} // namespace RoboidControl } // namespace RoboidControl

11
Things/Motor.h
View File

@ -6,18 +6,19 @@ namespace RoboidControl {
class Motor : public Thing { class Motor : public Thing {
public: public:
Motor(Participant* owner); Motor(Thing* parent = Thing::LocalRoot());
// Motor(Thing* parent);
Motor(Thing& parent = Thing::LocalRoot());
/// @brief Motor turning direction /// @brief Motor turning direction
enum class Direction { Clockwise = 1, CounterClockwise = -1 }; enum class Direction { Clockwise = 1, CounterClockwise = -1 };
/// @brief The forward turning direction of the motor /// @brief The forward turning direction of the motor
Direction direction; Direction direction;
virtual void SetTargetVelocity(float velocity); // -1..0..1 virtual void SetTargetVelocity(float velocity); // -1..0..1
protected: int GenerateBinary(char* bytes, unsigned char* ix) override;
// virtual void ProcessBinary(char* bytes) override;
//protected:
float targetVelocity = 0; float targetVelocity = 0;
}; };

2
Things/RelativeEncoder.cpp
View File

@ -2,7 +2,7 @@
namespace RoboidControl { namespace RoboidControl {
RelativeEncoder::RelativeEncoder(Thing& parent) RelativeEncoder::RelativeEncoder(Thing* parent)
: Thing(Type::IncrementalEncoder, parent) {} : Thing(Type::IncrementalEncoder, parent) {}
float RelativeEncoder::GetRotationSpeed() { float RelativeEncoder::GetRotationSpeed() {

2
Things/RelativeEncoder.h
View File

@ -16,7 +16,7 @@ class RelativeEncoder : public Thing {
/// rotation /// rotation
RelativeEncoder(Participant* owner); RelativeEncoder(Participant* owner);
// RelativeEncoder(Thing* parent); // RelativeEncoder(Thing* parent);
RelativeEncoder(Thing& parent = Thing::LocalRoot()); RelativeEncoder(Thing* parent = Thing::LocalRoot());
/// @brief Get the rotation speed /// @brief Get the rotation speed
/// @return The speed in revolutions per second /// @return The speed in revolutions per second

2
Things/TemperatureSensor.cpp
View File

@ -10,7 +10,7 @@ namespace RoboidControl {
// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} // TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
TemperatureSensor::TemperatureSensor(Thing& parent) : Thing(Type::TemperatureSensor, parent) {} TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {} // TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}

2
Things/TemperatureSensor.h
View File

@ -14,7 +14,7 @@ class TemperatureSensor : public Thing {
/// @param thingId The ID of the thing /// @param thingId The ID of the thing
TemperatureSensor(Participant* participant); //, unsigned char thingId); TemperatureSensor(Participant* participant); //, unsigned char thingId);
// TemperatureSensor(Thing* parent); // TemperatureSensor(Thing* parent);
TemperatureSensor(Thing& parent = Thing::LocalRoot()); TemperatureSensor(Thing* parent = Thing::LocalRoot());
/// @brief The measured temperature /// @brief The measured temperature
float temperature = 0; float temperature = 0;

20
Things/TouchSensor.cpp
View File

@ -2,15 +2,29 @@
namespace RoboidControl { namespace RoboidControl {
TouchSensor::TouchSensor(Thing& parent) : Thing(Type::TouchSensor, parent) {} TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) {
this->name = "Touch sensor";
}
void TouchSensor::PrepareForUpdate() {
this->touchedSomething = this->externalTouch;
}
void TouchSensor::Update(bool recursive) {
Thing::Update(recursive);
}
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
bytes[(*ix)++] = touchedSomething ? 1 : 0; bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
return 1; return 1;
} }
void TouchSensor::ProcessBinary(char* bytes) { void TouchSensor::ProcessBinary(char* bytes) {
this->touchedSomething |= (bytes[0] == 1); this->externalTouch = (bytes[0] == 1);
if (this->externalTouch)
std::cout << "touching!\n";
else
std::cout << "not touching\n";
} }
} // namespace RoboidControl } // namespace RoboidControl

7
Things/TouchSensor.h
View File

@ -12,12 +12,15 @@ class TouchSensor : public Thing {
/// @param parent The parent thing /// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate /// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID /// an ID
TouchSensor(Thing& parent = Thing::LocalRoot()); TouchSensor(Thing* parent = Thing::LocalRoot());
/// @brief Value which is true when the sensor is touching something, false /// @brief Value which is true when the sensor is touching something, false
/// otherwise /// otherwise
bool touchedSomething = false; bool touchedSomething = false;
virtual void PrepareForUpdate() override;
virtual void Update(bool recursive) override;
/// @brief Function used to generate binary data for this touch sensor /// @brief Function used to generate binary data for this touch sensor
/// @param buffer The byte array for thw binary data /// @param buffer The byte array for thw binary data
/// @param ix The starting position for writing the binary data /// @param ix The starting position for writing the binary data
@ -25,6 +28,8 @@ class TouchSensor : public Thing {
/// @brief Function used to process binary data received for this touch sensor /// @brief Function used to process binary data received for this touch sensor
/// @param bytes The binary data to process /// @param bytes The binary data to process
virtual void ProcessBinary(char* bytes) override; virtual void ProcessBinary(char* bytes) override;
protected:
bool externalTouch = false;
}; };
} // namespace RoboidControl } // namespace RoboidControl