diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index dad27fb..a787d5a 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -82,19 +82,6 @@ void ParticipantUDP::Receive() { senderAddress.toCharArray(sender_ipAddress, 16); 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); packetSize = udp->parsePacket(); } @@ -107,6 +94,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { // << remoteParticipant->port << "\n"; int n = 0; + int r = 0; do { if (n > 0) { #if !defined(NO_STD) @@ -115,9 +103,13 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { delay(10); } n++; + udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); 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 return true; diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index e95d94a..59a657d 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -7,14 +7,14 @@ namespace Arduino { #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; if (pinStandby != 255) 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->motorB = new DRV8833Motor(*this, config.BIn1, config.BIn2); + this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2); this->motorB->SetName("Motor B"); } @@ -23,11 +23,11 @@ DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) { #pragma region Differential drive DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, - Thing& parent) - : RoboidControl::DifferentialDrive(parent), drv8833(config) { - this->leftWheel = this->drv8833.motorA; - this->rightWheel = this->drv8833.motorB; -} + Thing* parent) + : RoboidControl::DifferentialDrive(this->drv8833.motorA, + this->drv8833.motorB, + parent), + drv8833(config, this) {} void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); @@ -42,7 +42,7 @@ void DRV8833::DifferentialDrive::Update(bool recurse) { uint8_t DRV8833Motor::nextAvailablePwmChannel = 0; #endif -DRV8833Motor::DRV8833Motor(DRV8833& driver, +DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 1f87b85..f2c2d3f 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -18,12 +18,11 @@ class DRV8833 : public Thing { int AIn2; int BIn1; int BIn2; - int standby = 255; + int standby; }; /// @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* motorB = nullptr; @@ -39,9 +38,7 @@ class DRV8833 : public Thing { class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { public: - // DifferentialDrive(DRV8833::Configuration config, - // Participant* participant = nullptr); - DifferentialDrive(DRV8833::Configuration config, Thing& parent = Thing::LocalRoot()); + DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot()); 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 pinIn2 the pin number for the in2 signal /// @param direction the forward turning direction of the motor - DRV8833Motor(DRV8833& driver, + DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index e6435db..12f3b12 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -7,7 +7,7 @@ namespace Arduino { #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; pinMode(this->pin, INPUT); std::cout << "digital input start\n"; @@ -23,7 +23,7 @@ void DigitalInput::Update(bool recursive) { #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) {} void DigitalInput::TouchSensor::Update(bool recursive) { @@ -39,7 +39,7 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0; volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0; DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, - Thing& parent) + Thing* parent) : RoboidControl::RelativeEncoder(parent), digitalInput(config.pin, parent), pulsesPerRevolution(config.pulsesPerRevolution) {} diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index 81a203e..e529dbc 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -12,9 +12,9 @@ class DigitalInput : public Thing { /// @brief Create a new digital input /// @param participant The participant to use /// @param pin The digital pin - DigitalInput(Participant* participant, unsigned char pin); + //DigitalInput(Participant* participant, 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 isLow = false; @@ -35,7 +35,7 @@ class DigitalInput : public Thing { class DigitalInput::TouchSensor : public RoboidControl::TouchSensor { public: - TouchSensor(unsigned char pin, Thing& parent); + TouchSensor(unsigned char pin, Thing* parent); /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) virtual void Update(bool recurse = false) override; @@ -55,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { unsigned char pulsesPerRevolution; }; - RelativeEncoder(Configuration config, Thing& parent = Thing::LocalRoot()); + RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot()); unsigned char pulsesPerRevolution; diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 81d581d..d92da44 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -6,8 +6,9 @@ namespace RoboidControl { namespace Arduino { -UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent) - : Thing(parent) { +UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) + : Thing(Type::Undetermined, parent) { + this->name = "Ultrasonic sensor"; this->pinTrigger = config.triggerPin; this->pinEcho = config.echoPin; @@ -57,16 +58,14 @@ void UltrasonicSensor::Update(bool recursive) { #pragma region Touch sensor -UltrasonicSensor::TouchSensor::TouchSensor( - UltrasonicSensor::Configuration config, - Thing& parent) - : RoboidControl::TouchSensor(parent), ultrasonic(config) {} +UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) + : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {} 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); + this->ultrasonic.Update(false); + this->touchedSomething |= (this->ultrasonic.distance > 0 && + this->ultrasonic.distance <= this->touchDistance); } #pragma region Touch sensor diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index e4554e2..7c02778 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -13,7 +13,7 @@ class UltrasonicSensor : Thing { int echoPin; }; - UltrasonicSensor(Configuration config, Thing& parent = Thing::LocalRoot()); + UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot()); // parameters @@ -47,7 +47,7 @@ class UltrasonicSensor : Thing { class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { public: TouchSensor(UltrasonicSensor::Configuration config, - Thing& parent = Thing::LocalRoot()); + Thing* parent = Thing::LocalRoot()); float touchDistance = 0.2f; diff --git a/Messages/ThingMsg.cpp b/Messages/ThingMsg.cpp index 2350dd2..be0c41c 100644 --- a/Messages/ThingMsg.cpp +++ b/Messages/ThingMsg.cpp @@ -17,19 +17,11 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) { if (thing->IsRoot()) this->parentId = 0; else { - Thing parent = thing->GetParent(); - this->parentId = parent.id; + Thing* parent = thing->GetParent(); + 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() {} unsigned char ThingMsg::Serialize(char* buffer) { diff --git a/Participant.cpp b/Participant.cpp index eea7563..3f11839 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -8,14 +8,26 @@ namespace RoboidControl { ParticipantRegistry Participant::registry; -//Participant Participant::LocalParticipant = Participant("0.0.0.0", 0); +Participant* Participant::LocalParticipant = new Participant(); -Participant& Participant::LocalParticipant() { - static Participant localParticipant("0.0.0.0", 0); - return localParticipant; - } +void Participant::ReplaceLocalParticipant(Participant& newParticipant) { + LocalParticipant = &newParticipant; + 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) { + // 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 int addressLength = (int)strlen(ipAddress); int stringLength = addressLength + 1; @@ -33,7 +45,7 @@ Participant::Participant(const char* ipAddress, int port) { } Participant::~Participant() { - registry.Remove(this); + // registry.Remove(this); delete[] this->ipAddress; } @@ -56,9 +68,7 @@ Thing* Participant::Get(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; @@ -76,7 +86,7 @@ void Participant::Add(Thing* thing, bool checkId) { this->things.push_back(thing); #endif // std::cout << "Add thing with generated ID " << this->ipAddress << ":" - // << this->port << "[" << (int)thing->id << "]\n"; + // << this->port << "[" << (int)thing->id << "]\n"; } else { Thing* foundThing = Get(thing->id); if (foundThing == nullptr) { @@ -128,8 +138,8 @@ Participant* ParticipantRegistry::Get(const char* ipAddress, continue; if (strcmp(participant->ipAddress, ipAddress) == 0 && participant->port == port) { - std::cout << "found participant " << participant->ipAddress << ":" - << (int)participant->port << std::endl; + // std::cout << "found participant " << participant->ipAddress << ":" + // << (int)participant->port << std::endl; return participant; } } @@ -140,7 +150,7 @@ Participant* ParticipantRegistry::Get(const char* ipAddress, } Participant* ParticipantRegistry::Get(unsigned char participantId) { -#if !defined(NO_STD) +#if !defined(NO_STD) for (Participant* participant : ParticipantRegistry::participants) { if (participant == nullptr) continue; @@ -148,7 +158,7 @@ Participant* ParticipantRegistry::Get(unsigned char participantId) { return participant; } std::cout << "Could not find participant " << (int)participantId << std::endl; -#endif +#endif return nullptr; } @@ -165,19 +175,22 @@ void ParticipantRegistry::Add(Participant* participant) { if (foundParticipant == nullptr) { #if defined(NO_STD) - //this->things[this->thingCount++] = thing; + // this->things[this->thingCount++] = thing; #else ParticipantRegistry::participants.push_back(participant); #endif - // std::cout << "Add participant " << participant->ipAddress << ":" - // << participant->port << "[" << (int)participant->networkId - // << "]\n"; - // std::cout << "participants " << ParticipantRegistry::participants.size() - // << "\n"; - // } else { - // std::cout << "Did not add, existing participant " << participant->ipAddress - // << ":" << participant->port << "[" << (int)participant->networkId - // << "]\n"; + // std::cout << "Add participant " << participant->ipAddress << ":" + // << participant->port << "[" << (int)participant->networkId + // << "]\n"; + // std::cout << "participants " << + // ParticipantRegistry::participants.size() + // << "\n"; + // } else { + // std::cout << "Did not add, existing participant " << + // participant->ipAddress + // << ":" << participant->port << "[" << + // (int)participant->networkId + // << "]\n"; } } diff --git a/Participant.h b/Participant.h index 2be1323..ea1cdcf 100644 --- a/Participant.h +++ b/Participant.h @@ -1,4 +1,5 @@ #pragma once + #include "Thing.h" namespace RoboidControl { @@ -36,6 +37,7 @@ class ParticipantRegistry { public: Participant** GetAll() const; int count = 0; + private: Participant** participants; #else @@ -43,6 +45,7 @@ class ParticipantRegistry { /// @brief Get all participants /// @return All participants const std::list& GetAll() const; + private: /// @brief The list of known participants std::list participants; @@ -57,6 +60,9 @@ class ParticipantRegistry { /// reference to remote participants. class Participant { public: + /// @brief The name of the participant + const char* name = "Participant"; + /// @brief The Ip Address of a participant. const char* ipAddress = "0.0.0.0"; /// @brief The port number for UDP communication with the participant. @@ -65,6 +71,7 @@ class Participant { /// @brief The network Id to identify the participant unsigned char networkId = 0; + Participant(); /// @brief Create a new participant with the given communcation info /// @param ipAddress The IP address of the participant /// @param port The UDP port of the participant @@ -72,7 +79,10 @@ class Participant { /// @brief Destructor for the participant ~Participant(); - static Participant& LocalParticipant(); + static Participant* LocalParticipant; + static void ReplaceLocalParticipant(Participant& newParticipant); + + Thing* root = new Thing(this); public: #if defined(NO_STD) diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 5aa4142..2ec2f88 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -1,5 +1,6 @@ #include "ParticipantUDP.h" +#include "Participant.h" #include "Thing.h" #include "Arduino/ArduinoParticipant.h" @@ -11,20 +12,38 @@ namespace RoboidControl { +#pragma region Init + ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { + this->name = "ParticipantUDP"; this->remoteSite = nullptr; if (this->port == 0) this->isIsolated = true; 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) : Participant("127.0.0.1", localPort) { + this->name = "ParticipantUDP"; if (this->port == 0) this->isIsolated = true; else this->remoteSite = new Participant(ipAddress, port); 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; @@ -64,11 +83,19 @@ void ParticipantUDP::SetupUDP(int localPort, this->connected = true; } +#pragma endregion Init + #pragma region Update +// The update order +// 1. receive external messages +// 2. update the state +// 3. send out the updated messages void ParticipantUDP::Update() { unsigned long currentTimeMs = Thing::GetTimeMs(); + PrepMyThings(); + if (this->isIsolated == false) { if (this->connected == false) begin(); @@ -91,11 +118,22 @@ void ParticipantUDP::Update() { UpdateOtherThings(); } +void ParticipantUDP::PrepMyThings() { + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + + thing->PrepareForUpdate(); + } +} + void ParticipantUDP::UpdateMyThings() { + // std::cout << this->things.size() << std::endl; for (Thing* thing : this->things) { if (thing == nullptr) // || thing->GetParent() != nullptr) continue; + // std::cout << thing->name << "\n"; if (thing->hierarchyChanged) { if (!(this->isIsolated || this->networkId == 0)) { ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); @@ -110,12 +148,14 @@ void ParticipantUDP::UpdateMyThings() { } } + // std::cout << "B\n"; // Why don't we do recursive? // Because when a thing creates a thing in the update, // that new thing is not sent out (because of hierarchyChanged) // before it is updated itself: it is immediatedly updated! thing->Update(false); + // std::cout << "C\n"; if (!(this->isIsolated || this->networkId == 0)) { if (thing->terminate) { DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); @@ -136,8 +176,10 @@ void ParticipantUDP::UpdateMyThings() { delete binaryMsg; } } + // std::cout << "D\n"; if (thing->terminate) this->Remove(thing); + // std::cout << "E\n"; } } @@ -200,8 +242,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { if (bufferSize <= 0) return true; - // std::cout << "send msg " << (int)this->buffer[0] << " to " - // << remoteParticipant->ipAddress << std::endl; + // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) + // << " to " << remoteParticipant->ipAddress << std::endl; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = @@ -378,14 +420,14 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { #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"; #endif 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); } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index c51131d..bf5e1a2 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -78,9 +78,6 @@ class ParticipantUDP : public Participant { /// local network long publishInterval = 3000; // 3 seconds - /// @brief The name of the participant - const char* name = "ParticipantUDP"; - protected: char buffer[1024]; @@ -105,6 +102,8 @@ class ParticipantUDP : public Participant { protected: unsigned long nextPublishMe = 0; + /// @brief Prepare the local things for the next update + virtual void PrepMyThings(); virtual void UpdateMyThings(); virtual void UpdateOtherThings(); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 394f328..9eb7ce9 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -73,10 +73,13 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); if (thing == nullptr) // 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) { - thing->SetParent(*Get(msg->parentId)); + thing->SetParent(Get(msg->parentId)); if (thing->IsRoot()) // if (thing->GetParent() != nullptr) #if defined(NO_STD) diff --git a/Thing.cpp b/Thing.cpp index 2ec0708..1ec24b3 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -20,12 +20,31 @@ namespace RoboidControl { #pragma region Init -Thing& Thing::LocalRoot() { - static Thing localRoot(Thing::Type::Undetermined, localRoot); +Thing* Thing::LocalRoot() { + Participant* p = Participant::LocalParticipant; + Thing* localRoot = p->root; 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->position = Spherical::zero; @@ -37,9 +56,16 @@ Thing::Thing(unsigned char thingType, Thing& parent) { this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; - this->owner = &Participant::LocalParticipant(); + this->owner = parent->owner; this->owner->Add(this, true); 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, @@ -66,28 +92,28 @@ void Thing::SetModel(const char* url) { #pragma region Hierarchy -// void Thing::SetParent(Thing* parent) { -// if (parent == nullptr) { -// Thing* parentThing = this->parent; -// if (parentThing != nullptr) -// parentThing->RemoveChild(this); -// this->parent = nullptr; -// } else -// parent->AddChild(this); -// this->hierarchyChanged = true; -// } - -void Thing::SetParent(Thing& parent) { - parent.AddChild(this); +void Thing::SetParent(Thing* parent) { + if (parent == nullptr) { + Thing* parentThing = this->parent; + if (parentThing != nullptr) + parentThing->RemoveChild(this); + this->parent = nullptr; + } else + parent->AddChild(this); this->hierarchyChanged = true; } -const Thing& Thing::GetParent() { - return *this->parent; -} +// void Thing::SetParent(Thing* parent) { +// parent->AddChild(this); +// this->hierarchyChanged = true; +// } + +// const Thing& Thing::GetParent() { +// return *this->parent; +// } 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) { @@ -96,9 +122,9 @@ bool Thing::IsRoot() const { // this->SetParent(thing); // } -// Thing* Thing::GetParent() { -// return this->parent; -// } +Thing* Thing::GetParent() { + return this->parent; +} Thing* Thing::GetChildByIndex(unsigned char ix) { return this->children[ix]; @@ -110,11 +136,11 @@ void Thing::AddChild(Thing* child) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { newChildren[childIx] = this->children[childIx]; - // if (this->children[childIx] == child) { - // // child is already present, stop copying do not update the children - // delete[] newChildren; - // return; - // } + if (this->children[childIx] == child) { + // child is already present, stop copying do not update the children + delete[] newChildren; + return; + } } newChildren[this->childCount] = child; @@ -143,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) { } } - child->parent = &Thing::LocalRoot(); + child->parent = Thing::LocalRoot(); delete[] this->children; this->children = newChildren; @@ -248,6 +274,8 @@ unsigned long Thing::GetTimeMs() { // Update(GetTimeMs(), recursive); // } +void Thing::PrepareForUpdate() {} + void Thing::Update(bool recursive) { // if (this->positionUpdated || this->orientationUpdated) // OnPoseChanged callback diff --git a/Thing.h b/Thing.h index 2fca59c..439606c 100644 --- a/Thing.h +++ b/Thing.h @@ -41,8 +41,15 @@ class Thing { }; #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 /// @param thingType The type of thing (can use Thing::Type) /// @param parent (optional) The parent thing @@ -50,7 +57,7 @@ class Thing { /// be Participant::LocalParticipant if the parent is not specified. A thing /// without a parent will be a root thing. Thing(unsigned char thingType = Thing::Type::Undetermined, - Thing& parent = LocalRoot()); + Thing* parent = LocalRoot()); /// @brief Create a new child thing /// @param parent The parent thing @@ -59,6 +66,8 @@ class Thing { /// an ID /// @note The owner will be the same as the owner of the parent thing + ~Thing(); + static Thing Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId); @@ -73,6 +82,7 @@ class Thing { /// @brief The participant managing this thing Participant* owner = nullptr; + /// @brief The ID of the thing unsigned char id = 0; @@ -108,11 +118,11 @@ class Thing { /// @brief Sets the parent of this Thing /// @param parent The Thing which should become the parent // virtual void SetParent(Thing* parent); - void SetParent(Thing& parent); + void SetParent(Thing* parent); /// @brief Gets the parent of this Thing /// @return The parent Thing // Thing* GetParent(); - const Thing& GetParent(); + Thing* GetParent(); bool IsRoot() const; @@ -215,12 +225,7 @@ class Thing { #pragma region Update public: - /// @brief Get the current time in milliseconds - /// @return The current time in milliseconds - static unsigned long GetTimeMs(); - - /// @brief Updates the state of the thing - // void Update(bool recursive = false); + virtual void PrepareForUpdate(); /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds; if this is @@ -230,6 +235,10 @@ class Thing { static void UpdateThings(); + /// @brief Get the current time in milliseconds + /// @return The current time in milliseconds + static unsigned long GetTimeMs(); + #pragma endregion Update public: diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp index 9cb2107..1eb1417 100644 --- a/Things/ControlledMotor.cpp +++ b/Things/ControlledMotor.cpp @@ -2,9 +2,9 @@ namespace RoboidControl { -ControlledMotor::ControlledMotor(Motor& motor, - RelativeEncoder& encoder, - Thing& parent) +ControlledMotor::ControlledMotor(Motor* motor, + RelativeEncoder* encoder, + Thing* parent) : Motor(parent), motor(motor), encoder(encoder) { this->type = Type::ControlledMotor; // encoder.SetParent(*this); @@ -19,9 +19,9 @@ void ControlledMotor::SetTargetVelocity(float velocity) { } 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(); float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f; @@ -31,10 +31,10 @@ void ControlledMotor::Update(bool recurse) { float acceleration = error * timeStep * pidP; // Just P is used at this moment std::cout << "motor acc. " << acceleration << std::endl; - motor.SetTargetVelocity(targetVelocity + + motor->SetTargetVelocity(targetVelocity + acceleration); // or something like that - motor.Update(false); + motor->Update(false); } // float ControlledMotor::GetActualVelocity() { diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h index 1f46aa2..462f206 100644 --- a/Things/ControlledMotor.h +++ b/Things/ControlledMotor.h @@ -10,7 +10,7 @@ namespace RoboidControl { /// The speed is measured in revolutions per second. class ControlledMotor : public Motor { public: - ControlledMotor(Motor& motor, RelativeEncoder& encoder, Thing& parent = Thing::LocalRoot()); + ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot()); float pidP = 1; float pidD = 0; @@ -28,8 +28,8 @@ class ControlledMotor : public Motor { /// @param speed the target velocity in revolutions per second virtual void SetTargetVelocity(float velocity) override; - Motor& motor; - RelativeEncoder& encoder; + Motor* motor; + RelativeEncoder* encoder; protected: float lastUpdateTime; diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index bde68c7..14967ae 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -1,11 +1,27 @@ #include "DifferentialDrive.h" +#include "Messages/LowLevelMessages.h" + namespace RoboidControl { -DifferentialDrive::DifferentialDrive(Thing& parent) +DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(Type::DifferentialDrive, parent) { - this->leftWheel = new Motor(); - this->rightWheel = new Motor(); + this->name = "Differential drive"; + + 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, @@ -23,13 +39,13 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter, this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -Motor& DifferentialDrive::GetMotorLeft() { - return *this->leftWheel; -} +// Motor& DifferentialDrive::GetMotorLeft() { +// return *this->leftWheel; +// } -Motor& DifferentialDrive::GetMotorRight() { - return *this->rightWheel; -} +// Motor& DifferentialDrive::GetMotorRight() { +// return *this->rightWheel; +// } void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) { float distance = this->wheelSeparation / 2; @@ -78,4 +94,11 @@ void DifferentialDrive::Update(bool 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 \ No newline at end of file diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index c7685fe..cb45977 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -14,7 +14,9 @@ class DifferentialDrive : public Thing { /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// 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 /// @param wheelDiameter The diameter of the wheels in meters @@ -25,8 +27,8 @@ class DifferentialDrive : public Thing { /// @sa SetLinearVelocity SetAngularVelocity void SetDriveDimensions(float wheelDiameter, float wheelSeparation); - Motor& GetMotorLeft(); - Motor& GetMotorRight(); + // Motor& GetMotorLeft(); + // Motor& GetMotorRight(); /// @brief Congures the motors for the wheels /// @param leftWheel The motor for the left wheel /// @param rightWheel The motor for the right wheel @@ -42,20 +44,23 @@ class DifferentialDrive : public Thing { /// @copydoc RoboidControl::Thing::Update(unsigned long) 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: /// @brief The radius of a wheel in meters - float wheelRadius = 1.0f; + float wheelRadius = 0.0f; /// @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 float rpsToMs = 1.0f; - /// @brief The left wheel - Motor* leftWheel = nullptr; - /// @brief The right wheel - Motor* rightWheel = nullptr; }; } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 0c1e213..ba0d859 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -13,7 +13,7 @@ namespace RoboidControl { // DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, 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) { bytes[(*ix)++] = state ? 1 : 0; diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index b6579f2..e31e87d 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -19,7 +19,7 @@ class DigitalSensor : public Thing { /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID // DigitalSensor(Thing* parent); //, unsigned char thingId = 0); - DigitalSensor(Thing& parent = Thing::LocalRoot()); + DigitalSensor(Thing* parent = Thing::LocalRoot()); /// @brief The sigital state bool state = 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index 354c7bd..12ab226 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -2,16 +2,15 @@ namespace RoboidControl { -// RoboidControl::Motor::Motor(Participant* owner) -// : Thing(owner, Type::UncontrolledMotor) {} +Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {} -// RoboidControl::Motor::Motor(Thing* parent) -// : Thing(parent, Type::UncontrolledMotor) {} - -Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {} - -void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) { +void Motor::SetTargetVelocity(float targetSpeed) { this->targetVelocity = targetSpeed; } +int Motor::GenerateBinary(char* data, unsigned char* ix) { + data[(*ix)++] = this->targetVelocity * 127.0f; + return 1; +} + } // namespace RoboidControl \ No newline at end of file diff --git a/Things/Motor.h b/Things/Motor.h index a625bb3..38ac149 100644 --- a/Things/Motor.h +++ b/Things/Motor.h @@ -6,18 +6,19 @@ namespace RoboidControl { class Motor : public Thing { public: - Motor(Participant* owner); - // Motor(Thing* parent); - Motor(Thing& parent = Thing::LocalRoot()); + Motor(Thing* parent = Thing::LocalRoot()); /// @brief Motor turning direction enum class Direction { Clockwise = 1, CounterClockwise = -1 }; /// @brief The forward turning direction of the motor 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; }; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index 5cc9464..6e6a19c 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -2,7 +2,7 @@ namespace RoboidControl { -RelativeEncoder::RelativeEncoder(Thing& parent) +RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(Type::IncrementalEncoder, parent) {} float RelativeEncoder::GetRotationSpeed() { diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index e783c7d..b2aeda4 100644 --- a/Things/RelativeEncoder.h +++ b/Things/RelativeEncoder.h @@ -16,7 +16,7 @@ class RelativeEncoder : public Thing { /// rotation RelativeEncoder(Participant* owner); // RelativeEncoder(Thing* parent); - RelativeEncoder(Thing& parent = Thing::LocalRoot()); + RelativeEncoder(Thing* parent = Thing::LocalRoot()); /// @brief Get the rotation speed /// @return The speed in revolutions per second diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index a3fb13d..d67800b 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -10,7 +10,7 @@ namespace RoboidControl { // 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) {} diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index b1feac3..0da4210 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -14,7 +14,7 @@ class TemperatureSensor : public Thing { /// @param thingId The ID of the thing TemperatureSensor(Participant* participant); //, unsigned char thingId); // TemperatureSensor(Thing* parent); - TemperatureSensor(Thing& parent = Thing::LocalRoot()); + TemperatureSensor(Thing* parent = Thing::LocalRoot()); /// @brief The measured temperature float temperature = 0; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 6960b45..d1b9b41 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,15 +2,29 @@ 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) { - bytes[(*ix)++] = touchedSomething ? 1 : 0; + bytes[(*ix)++] = this->touchedSomething ? 1 : 0; return 1; } 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 \ No newline at end of file diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 0768f66..cd7bd73 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -12,12 +12,15 @@ class TouchSensor : public Thing { /// @param parent The parent thing /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - TouchSensor(Thing& parent = Thing::LocalRoot()); + TouchSensor(Thing* parent = Thing::LocalRoot()); /// @brief Value which is true when the sensor is touching something, false /// otherwise bool touchedSomething = false; + virtual void PrepareForUpdate() override; + virtual void Update(bool recursive) override; + /// @brief Function used to generate binary data for this touch sensor /// @param buffer The byte array for thw 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 /// @param bytes The binary data to process virtual void ProcessBinary(char* bytes) override; +protected: + bool externalTouch = false; }; } // namespace RoboidControl