From 1b5fef15e6d31446813e2fcc66c4a62a2cfb8dba Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 19 May 2025 15:58:35 +0200 Subject: [PATCH] More refactoring to get rid of pointers --- Arduino/Things/DRV8833.cpp | 32 ++++------ Arduino/Things/DRV8833.h | 15 ++--- Arduino/Things/DigitalInput.cpp | 30 ++++----- Arduino/Things/DigitalInput.h | 7 ++- Arduino/Things/UltrasonicSensor.cpp | 23 +------ Arduino/Things/UltrasonicSensor.h | 11 +--- Messages/PoseMsg.cpp | 4 +- Messages/ThingMsg.cpp | 14 +++-- Participant.cpp | 11 +++- Participant.h | 2 +- Participants/SiteServer.cpp | 13 ++-- Thing.cpp | 95 +++++++++++++---------------- Thing.h | 25 ++++---- Things/ControlledMotor.cpp | 22 ++++--- Things/ControlledMotor.h | 24 ++------ Things/DifferentialDrive.cpp | 52 +++++++--------- Things/DifferentialDrive.h | 25 +++----- Things/DigitalSensor.cpp | 2 +- Things/DigitalSensor.h | 2 +- Things/Motor.cpp | 4 +- Things/Motor.h | 2 +- Things/RelativeEncoder.cpp | 2 - Things/RelativeEncoder.h | 2 +- Things/TemperatureSensor.cpp | 2 +- Things/TemperatureSensor.h | 2 +- Things/TouchSensor.cpp | 14 ----- Things/TouchSensor.h | 10 +-- 27 files changed, 181 insertions(+), 266 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 373bce1..e95d94a 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -7,41 +7,31 @@ namespace Arduino { #pragma region DRV8833 -DRV8833::DRV8833(Configuration config, Participant* participant) - : Thing(participant) { - this->pinStandby = pinStandby; +DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(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"); } -DRV8833::DRV8833(Configuration config, Thing* parent) - : DRV8833(config, parent->owner) { - this->SetParent(parent); -} - #pragma endregion DRV8833 #pragma region Differential drive -DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, - Participant* owner) - : RoboidControl::DifferentialDrive(owner), drv8833(config, owner) { - this->leftWheel = this->drv8833.motorA; - this->rightWheel = this->drv8833.motorB; -} - DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing& parent) - : RoboidControl::DifferentialDrive(parent), drv8833(config) {} + : RoboidControl::DifferentialDrive(parent), drv8833(config) { + this->leftWheel = this->drv8833.motorA; + this->rightWheel = this->drv8833.motorB; +} void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); - this->drv8833.Update(recurse); + this->drv8833.Update(false); } #pragma endregion Differential drive @@ -52,11 +42,11 @@ 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) - : Motor(driver->owner) { + : Motor() { this->SetParent(driver); this->pinIn1 = pinIn1; diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index c13a333..1f87b85 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -18,11 +18,12 @@ class DRV8833 : public Thing { int AIn2; int BIn1; int BIn2; + int standby = 255; }; /// @brief Setup a DRV8833 motor controller - DRV8833(Configuration config, Participant* owner = nullptr); - DRV8833(Configuration config, Thing* parent); + // DRV8833(Configuration config, Participant* owner = nullptr); + DRV8833(Configuration config, Thing& parent = Thing::LocalRoot()); DRV8833Motor* motorA = nullptr; DRV8833Motor* motorB = nullptr; @@ -38,9 +39,9 @@ class DRV8833 : public Thing { class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { public: - DifferentialDrive(DRV8833::Configuration config, - Participant* participant = nullptr); - DifferentialDrive(DRV8833::Configuration config, Thing& parent); + // DifferentialDrive(DRV8833::Configuration config, + // Participant* participant = nullptr); + DifferentialDrive(DRV8833::Configuration config, Thing& parent = Thing::LocalRoot()); virtual void Update(bool recurse = false) override; @@ -59,7 +60,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); @@ -81,7 +82,7 @@ class DRV8833Motor : public Motor { #endif }; -#pragma endregion Moto +#pragma endregion Motor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index 64486cc..e6435db 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -7,28 +7,15 @@ namespace Arduino { #pragma region Digital input -DigitalInput::DigitalInput(Participant* participant, unsigned char pin) - : Thing(participant) { - this->pin = pin; - - pinMode(this->pin, INPUT); -} - DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) { this->pin = pin; pinMode(this->pin, INPUT); + std::cout << "digital input start\n"; } -// DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) { -// this->pin = pin; - -// pinMode(this->pin, INPUT); -// } - void DigitalInput::Update(bool recursive) { this->isHigh = digitalRead(this->pin); this->isLow = !this->isHigh; - // std::cout << "DigitalINput pin " << (int)this->pin << ": " << Thing::Update(recursive); } @@ -55,13 +42,16 @@ DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, Thing& parent) : RoboidControl::RelativeEncoder(parent), digitalInput(config.pin, parent), - pulsesPerRevolution(config.pulsesPerRevolution) { + pulsesPerRevolution(config.pulsesPerRevolution) {} +void DigitalInput::RelativeEncoder::Start() { // We support up to 2 pulse counters if (interruptCount == 0) - attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt0, RISING); + attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0, + RISING); else if (interruptCount == 1) - attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt1, RISING); + attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1, + RISING); else { // maximum interrupt count reached std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached" @@ -70,6 +60,7 @@ DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, } interruptIx = interruptCount; interruptCount++; + std::cout << "pin ints. " << interruptCount << std::endl; } int DigitalInput::RelativeEncoder::GetPulseCount() { @@ -88,6 +79,7 @@ int DigitalInput::RelativeEncoder::GetPulseCount() { void DigitalInput::RelativeEncoder::Update(bool recursive) { unsigned long currentTimeMs = GetTimeMs(); if (this->lastUpdateTime == 0) { + this->Start(); this->lastUpdateTime = currentTimeMs; return; } @@ -99,8 +91,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) { this->pulseFrequency = pulseCount / timeStep; this->rotationSpeed = pulseFrequency / pulsesPerRevolution; - std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency - << " timestep " << timeStep << std::endl; + // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + // << " timestep " << timeStep << std::endl; this->lastUpdateTime = currentTimeMs; } diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index ef3d25c..81a203e 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -13,7 +13,7 @@ class DigitalInput : public Thing { /// @param participant The participant to use /// @param pin The digital 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); bool isHigh = false; @@ -55,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { unsigned char pulsesPerRevolution; }; - RelativeEncoder(Configuration config, Thing& parent); + RelativeEncoder(Configuration config, Thing& parent = Thing::LocalRoot()); unsigned char pulsesPerRevolution; @@ -81,6 +81,9 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { int GetPulseCount(); long netPulseCount = 0; unsigned long lastUpdateTime = 0; + + private: + void Start(); }; #pragma endregion Incremental encoder diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 7abd9a8..81d581d 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -6,16 +6,6 @@ namespace RoboidControl { namespace Arduino { -UltrasonicSensor::UltrasonicSensor(Configuration config, - Participant* participant) - : Thing(participant) { - this->pinTrigger = config.triggerPin; - this->pinEcho = config.echoPin; - - pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode - pinMode(pinEcho, INPUT); // configure the echo pin to input mode -} - UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent) : Thing(parent) { this->pinTrigger = config.triggerPin; @@ -67,23 +57,16 @@ void UltrasonicSensor::Update(bool recursive) { #pragma region Touch sensor -// UltrasonicSensor::TouchSensor::TouchSensor( -// UltrasonicSensor::Configuration config, -// Thing& parent) -// : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) { -// this->touchedSomething = false; -// } UltrasonicSensor::TouchSensor::TouchSensor( UltrasonicSensor::Configuration config, Thing& parent) - : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) { - this->touchedSomething = false; -} + : RoboidControl::TouchSensor(parent), ultrasonic(config) {} void UltrasonicSensor::TouchSensor::Update(bool recursive) { - this->ultrasonic.Update(recursive); + this->ultrasonic.Update(false); this->touchedSomething = (this->ultrasonic.distance > 0 && this->ultrasonic.distance <= this->touchDistance); + RoboidControl::TouchSensor::Update(recursive); } #pragma region Touch sensor diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 0ae252e..e4554e2 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -13,12 +13,7 @@ class UltrasonicSensor : Thing { int echoPin; }; - /// @brief Setup an ultrasonic sensor - /// @param participant The participant to use - /// @param pinTrigger The pin number of the trigger signal - /// @param pinEcho The pin number of the echo signal - UltrasonicSensor(Configuration config, Participant* participant); - UltrasonicSensor(Configuration config, Thing& parent); + UltrasonicSensor(Configuration config, Thing& parent = Thing::LocalRoot()); // parameters @@ -51,8 +46,8 @@ class UltrasonicSensor : Thing { class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { public: - TouchSensor(UltrasonicSensor::Configuration config, Thing& parent); - //TouchSensor(UltrasonicSensor::Configuration config, Thing* parent); + TouchSensor(UltrasonicSensor::Configuration config, + Thing& parent = Thing::LocalRoot()); float touchDistance = 0.2f; diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index c476388..79d2e4a 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || (force && thing->GetParent() != nullptr)) { + if (thing->positionUpdated || (force && thing->IsRoot())) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) { + if (thing->orientationUpdated || (force && thing->IsRoot())) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } diff --git a/Messages/ThingMsg.cpp b/Messages/ThingMsg.cpp index 932530f..2350dd2 100644 --- a/Messages/ThingMsg.cpp +++ b/Messages/ThingMsg.cpp @@ -14,11 +14,12 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; this->thingType = thing->type; - Thing* parent = thing->GetParent(); - if (parent != nullptr) - this->parentId = parent->id; - else + if (thing->IsRoot()) this->parentId = 0; + else { + Thing parent = thing->GetParent(); + this->parentId = parent.id; + } } // ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId, @@ -33,8 +34,9 @@ ThingMsg::~ThingMsg() {} unsigned char ThingMsg::Serialize(char* buffer) { #if defined(DEBUG) - std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId - << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl; + std::cout << "Send ThingMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << (int)this->thingType << " " + << (int)this->parentId << std::endl; #endif unsigned char ix = 0; buffer[ix++] = this->id; diff --git a/Participant.cpp b/Participant.cpp index 51dfabd..eea7563 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -8,7 +8,12 @@ namespace RoboidControl { ParticipantRegistry Participant::registry; -Participant LocalParticipant = Participant("0.0.0.0", 0); +//Participant Participant::LocalParticipant = Participant("0.0.0.0", 0); + +Participant& Participant::LocalParticipant() { + static Participant localParticipant("0.0.0.0", 0); + return localParticipant; + } Participant::Participant(const char* ipAddress, int port) { // make a copy of the ip address string @@ -51,7 +56,9 @@ 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; @@ -69,7 +76,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) { diff --git a/Participant.h b/Participant.h index 35c5ce4..2be1323 100644 --- a/Participant.h +++ b/Participant.h @@ -72,7 +72,7 @@ class Participant { /// @brief Destructor for the participant ~Participant(); - static Participant LocalParticipant; + static Participant& LocalParticipant(); public: #if defined(NO_STD) diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index a24bf09..394f328 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -27,7 +27,7 @@ void SiteServer::UpdateMyThings() { if (thing == nullptr) continue; - thing->Update( true); + thing->Update(true); if (this->isIsolated == false) { // Send to all other participants @@ -76,16 +76,17 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing::Reconstruct(sender, msg->thingType, msg->thingId); if (msg->parentId != 0) { - thing->SetParent(Get(msg->parentId)); - if (thing->GetParent() != nullptr) + thing->SetParent(*Get(msg->parentId)); + if (thing->IsRoot()) + // if (thing->GetParent() != nullptr) #if defined(NO_STD) ; -#else +#else std::cout << "Could not find parent [" << (int)msg->networkId << "/" << (int)msg->parentId << "]\n"; -#endif +#endif } else - thing->SetParent(nullptr); + thing->SetParent(Thing::LocalRoot()); } #pragma endregion Receive diff --git a/Thing.cpp b/Thing.cpp index 3f8168c..2ec0708 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -20,7 +20,10 @@ namespace RoboidControl { #pragma region Init -Thing Thing::Root = Thing(Thing::Type::Undetermined, Root); +Thing& Thing::LocalRoot() { + static Thing localRoot(Thing::Type::Undetermined, localRoot); + return localRoot; +} Thing::Thing(unsigned char thingType, Thing& parent) { this->type = thingType; @@ -34,47 +37,18 @@ Thing::Thing(unsigned char thingType, Thing& parent) { this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; - // std::cout << "add thing [" << (int)this->id << "] to owner " - // << this->owner->ipAddress << ":" << this->owner->port << - // std::endl; + this->owner = &Participant::LocalParticipant(); this->owner->Add(this, true); - this->SetParent(&parent); + this->SetParent(parent); } -Thing::Thing(Participant* owner, unsigned char thingType) { - // unsigned char thingId) { - if (owner == nullptr) - owner = IsolatedParticipant::Isolated(); - this->owner = owner; - // this->id = thingId; - this->type = thingType; - - 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; - - // std::cout << "add thing [" << (int)this->id << "] to owner " - // << this->owner->ipAddress << ":" << this->owner->port << - // std::endl; - this->owner->Add(this, true); -} - -// Thing::Thing(Thing* parent, unsigned char thingType) //, unsigned char thingId) -// : Thing(parent->owner, thingType) { //}, thingId) { -// this->SetParent(parent); +// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, +// unsigned char thingId) { +// Thing thing = Thing(owner, thingType); +// thing.id = thingId; +// return thing; // } -Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId) { - Thing thing = Thing(owner, thingType); - thing.id = thingId; - return thing; -} - #pragma endregion Init void Thing::SetName(const char* name) { @@ -92,26 +66,39 @@ 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); +// 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); this->hierarchyChanged = true; } +const Thing& Thing::GetParent() { + return *this->parent; +} + +bool Thing::IsRoot() const { + return this == &LocalRoot(); //&Thing::Root; +} + // void Thing::SetParent(Thing* root, const char* name) { // Thing* thing = root->FindChild(name); // if (thing != nullptr) // 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]; @@ -123,11 +110,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; @@ -156,7 +143,7 @@ Thing* Thing::RemoveChild(Thing* child) { } } - child->parent = nullptr; + child->parent = &Thing::LocalRoot(); delete[] this->children; this->children = newChildren; diff --git a/Thing.h b/Thing.h index cc97ab8..2fca59c 100644 --- a/Thing.h +++ b/Thing.h @@ -41,17 +41,16 @@ class Thing { }; #pragma region Init - static Thing Root; + static Thing& LocalRoot(); - Thing(unsigned char thingType = Thing::Type::Undetermined, Thing& parent = Root); - - /// @brief Create a new thing for a participant - /// @param owner The owning participant + /// @brief Create a new thing /// @param thingType The type of thing (can use Thing::Type) - /// @param thingId The ID of the thing, leave out or set to zero to generate - /// an ID - Thing(Participant* owner, - unsigned char thingType = Thing::Type::Undetermined); + /// @param parent (optional) The parent thing + /// The owner will be the same as the owner of the parent thing, it will + /// 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()); /// @brief Create a new child thing /// @param parent The parent thing @@ -108,10 +107,14 @@ class Thing { /// @brief Sets the parent of this Thing /// @param parent The Thing which should become the parent - virtual void SetParent(Thing* parent); + // virtual void SetParent(Thing* parent); + void SetParent(Thing& parent); /// @brief Gets the parent of this Thing /// @return The parent Thing - Thing* GetParent(); + // Thing* GetParent(); + const Thing& GetParent(); + + bool IsRoot() const; /// @brief The number of children unsigned char childCount = 0; diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp index bff3b80..9cb2107 100644 --- a/Things/ControlledMotor.cpp +++ b/Things/ControlledMotor.cpp @@ -2,11 +2,14 @@ namespace RoboidControl { -ControlledMotor::ControlledMotor(Motor* motor, RelativeEncoder* encoder) - : Motor(), motor(motor), encoder(encoder) { +ControlledMotor::ControlledMotor(Motor& motor, + RelativeEncoder& encoder, + Thing& parent) + : Motor(parent), motor(motor), encoder(encoder) { this->type = Type::ControlledMotor; - Thing* parent = motor->GetParent(); - this->SetParent(parent); + // encoder.SetParent(*this); + // Thing parent = motor.GetParent(); + // this->SetParent(parent); } void ControlledMotor::SetTargetVelocity(float velocity) { @@ -16,9 +19,9 @@ void ControlledMotor::SetTargetVelocity(float velocity) { } void ControlledMotor::Update(bool recurse) { - encoder->Update(recurse); + 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; @@ -27,10 +30,11 @@ void ControlledMotor::Update(bool recurse) { float error = this->targetVelocity - this->actualVelocity; float acceleration = error * timeStep * pidP; // Just P is used at this moment - motor->SetTargetVelocity(targetVelocity + - acceleration); // or something like that + std::cout << "motor acc. " << acceleration << std::endl; + motor.SetTargetVelocity(targetVelocity + + acceleration); // or something like that - motor->Update(recurse); + motor.Update(false); } // float ControlledMotor::GetActualVelocity() { diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h index 1742289..1f46aa2 100644 --- a/Things/ControlledMotor.h +++ b/Things/ControlledMotor.h @@ -10,12 +10,13 @@ namespace RoboidControl { /// The speed is measured in revolutions per second. class ControlledMotor : public Motor { public: - ControlledMotor(Motor* motor, RelativeEncoder* encoder); + ControlledMotor(Motor& motor, RelativeEncoder& encoder, Thing& parent = Thing::LocalRoot()); float pidP = 1; float pidD = 0; float pidI = 0; + /// @brief The actual velocity in revolutions per second float actualVelocity; enum Direction { Forward = 1, Reverse = -1 }; @@ -24,29 +25,14 @@ class ControlledMotor : public Motor { virtual void Update(bool recurse = false) override; /// @brief Set the target verlocity for the motor controller - /// @param speed the target velocity in revolutions per second. + /// @param speed the target velocity in revolutions per second virtual void SetTargetVelocity(float velocity) override; - /// @brief Get the actual velocity from the encoder - /// @return The velocity in revolutions per second - // float GetActualVelocity(); - - // bool Drive(float distance); - - Motor* motor; - RelativeEncoder* encoder; + Motor& motor; + RelativeEncoder& encoder; protected: float lastUpdateTime; - - - //float targetVelocity; - -// float netDistance = 0; -// float startDistance = 0; -// bool driving = false; -// float targetDistance = 0; -// float lastEncoderPosition = 0; }; } // namespace RoboidControl diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index 0b472a7..bde68c7 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -2,25 +2,8 @@ namespace RoboidControl { -// DifferentialDrive::DifferentialDrive(Participant* participant, -// unsigned char thingId) -// : Thing(participant, Thing::Type::DifferentialDrive, thingId) { -// this->leftWheel = new Motor(); -// this->rightWheel = new Motor(); -// } - -// DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId) -// : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} - -DifferentialDrive::DifferentialDrive(Participant* owner) : Thing(owner, Type::DifferentialDrive) { - this->leftWheel = new Motor(); - this->rightWheel = new Motor(); -} -// DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent, Type::DifferentialDrive) { -// this->leftWheel = new Motor(); -// this->rightWheel = new Motor(); -// } -DifferentialDrive::DifferentialDrive(Thing& parent) : Thing(Type::DifferentialDrive, parent) { +DifferentialDrive::DifferentialDrive(Thing& parent) + : Thing(Type::DifferentialDrive, parent) { this->leftWheel = new Motor(); this->rightWheel = new Motor(); } @@ -40,21 +23,28 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter, this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -// 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)); +Motor& DifferentialDrive::GetMotorLeft() { + return *this->leftWheel; +} -// this->rightWheel = rightWheel; -// if (rightWheel != nullptr) -// this->rightWheel->SetPosition(Spherical(distance, Direction::right)); -// } +Motor& DifferentialDrive::GetMotorRight() { + return *this->rightWheel; +} -void DifferentialDrive::SetWheelVelocity(float velocityLeft, float velocityRight) { +void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) { + float distance = this->wheelSeparation / 2; + this->leftWheel = &leftMotor; + this->leftWheel->SetPosition(Spherical(distance, Direction::left)); + + this->rightWheel = &rightMotor; + this->rightWheel->SetPosition(Spherical(distance, Direction::right)); +} + +void DifferentialDrive::SetWheelVelocity(float velocityLeft, + float velocityRight) { // if (this->leftWheel != nullptr) - // this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, Direction::left)); + // this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, + // Direction::left)); // if (this->rightWheel != nullptr) // this->rightWheel->SetAngularVelocity( // Spherical(velocityRight, Direction::right)); diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index 65cd5a7..c7685fe 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -10,20 +10,11 @@ namespace RoboidControl { /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink class DifferentialDrive : public Thing { public: - // /// @brief Create a differential drive without networking support - // DifferentialDrive(); - /// @brief Create a differential drive with networking support - /// @param participant The local participant - /// @param thingId The ID of the thing, leave out or set to zero to generate - /// an ID - DifferentialDrive(Participant* participant); //, - //unsigned char thingId = 0); /// @brief Create a new child differential drive /// @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); //, unsigned char thingId = 0); - DifferentialDrive(Thing& parent = Thing::Root); + DifferentialDrive(Thing& parent = Thing::LocalRoot()); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -33,10 +24,13 @@ class DifferentialDrive : public Thing { /// linear and angular velocity. /// @sa SetLinearVelocity SetAngularVelocity void SetDriveDimensions(float wheelDiameter, float wheelSeparation); + + 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 - void SetMotors(Thing* leftWheel, Thing* rightWheel); + void SetMotors(Motor& leftMotor, Motor& rightMotor); /// @brief Directly specify the speeds of the motors /// @param speedLeft The speed of the left wheel in degrees per second. @@ -48,10 +42,6 @@ class DifferentialDrive : public Thing { /// @copydoc RoboidControl::Thing::Update(unsigned long) virtual void Update(bool recursive = true) override; - /// @brief The left wheel - Motor* leftWheel = nullptr; - /// @brief The right wheel - Motor* rightWheel = nullptr; protected: /// @brief The radius of a wheel in meters @@ -61,6 +51,11 @@ class DifferentialDrive : public Thing { /// @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 0ac2414..0c1e213 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -10,7 +10,7 @@ namespace RoboidControl { // DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) // : Thing(parent, Type::Switch) {} -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(Type::Switch, parent) {} diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index 9b77f6b..b6579f2 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::Root); + DigitalSensor(Thing& parent = Thing::LocalRoot()); /// @brief The sigital state bool state = 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index bbcd532..354c7bd 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -2,8 +2,8 @@ namespace RoboidControl { -RoboidControl::Motor::Motor(Participant* owner) - : Thing(owner, Type::UncontrolledMotor) {} +// RoboidControl::Motor::Motor(Participant* owner) +// : Thing(owner, Type::UncontrolledMotor) {} // RoboidControl::Motor::Motor(Thing* parent) // : Thing(parent, Type::UncontrolledMotor) {} diff --git a/Things/Motor.h b/Things/Motor.h index 530941b..a625bb3 100644 --- a/Things/Motor.h +++ b/Things/Motor.h @@ -8,7 +8,7 @@ class Motor : public Thing { public: Motor(Participant* owner); // Motor(Thing* parent); - Motor(Thing& parent = Thing::Root); + Motor(Thing& parent = Thing::LocalRoot()); /// @brief Motor turning direction enum class Direction { Clockwise = 1, CounterClockwise = -1 }; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp index b7f8a8c..5cc9464 100644 --- a/Things/RelativeEncoder.cpp +++ b/Things/RelativeEncoder.cpp @@ -2,8 +2,6 @@ namespace RoboidControl { -RelativeEncoder::RelativeEncoder(Participant* owner) - : Thing(owner, Type::IncrementalEncoder) {} RelativeEncoder::RelativeEncoder(Thing& parent) : Thing(Type::IncrementalEncoder, parent) {} diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h index ff951b8..e783c7d 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::Root); + 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 d9c9113..a3fb13d 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -8,7 +8,7 @@ namespace RoboidControl { // unsigned char thingId) // : Thing(participant, Type::TemperatureSensor, thingId) {} -TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} +// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} TemperatureSensor::TemperatureSensor(Thing& parent) : Thing(Type::TemperatureSensor, parent) {} diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index 23f56fa..b1feac3 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::Root); + TemperatureSensor(Thing& parent = Thing::LocalRoot()); /// @brief The measured temperature float temperature = 0; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index d3d2a91..6960b45 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,20 +2,6 @@ namespace RoboidControl { -// TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} - -// TouchSensor::TouchSensor(Participant* owner, unsigned char thingId) -// : Thing(owner, Thing::Type::TouchSensor, thingId) {} - -// TouchSensor::TouchSensor(Thing* parent, unsigned char thingId) : -// Thing(parent->owner, Thing::Type::TouchSensor, thingId) { -// this->SetParent(parent); -// } - -TouchSensor::TouchSensor(Participant* owner) - : Thing(owner, Type::TouchSensor) {} - -// TouchSensor::TouchSensor(Thing* parent) : Thing(parent, Type::TouchSensor) {} TouchSensor::TouchSensor(Thing& parent) : Thing(Type::TouchSensor, parent) {} int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index aca28a4..0768f66 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -8,19 +8,11 @@ namespace RoboidControl { class TouchSensor : public Thing { // Why finishing this release (0.3), I notice that this is equivalent to a digital sensor public: - /// @brief Create a touch sensor without communication abilities - //TouchSensor(); - /// @brief Create a touch sensor for a participant - /// @param owner The owning participant - /// @param thingId The ID of the thing, leave out or set to zero to generate - /// an ID - TouchSensor(Participant* owner); //, unsigned char thingId = 0); /// @brief Create a new child touch sensor /// @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); //, unsigned char thingId = 0); - TouchSensor(Thing& parent = Thing::Root); + TouchSensor(Thing& parent = Thing::LocalRoot()); /// @brief Value which is true when the sensor is touching something, false /// otherwise