From 17de9db0be248697baf1b3935cbf25927bac50c5 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 16 May 2025 11:54:40 +0200 Subject: [PATCH] Added rel. encoder, removed currentTime in Update() --- Arduino/Things/DRV8833.cpp | 29 ++----- Arduino/Things/DRV8833.h | 14 ++-- Arduino/Things/DigitalInput.cpp | 117 ++++++++++++++++++++++++++-- Arduino/Things/DigitalInput.h | 68 +++++++++++++++- Arduino/Things/UltrasonicSensor.cpp | 11 ++- Arduino/Things/UltrasonicSensor.h | 6 +- Participant.cpp | 4 +- Participant.h | 2 +- Participants/ParticipantUDP.cpp | 25 +++--- Participants/ParticipantUDP.h | 6 +- Participants/SiteServer.cpp | 4 +- Participants/SiteServer.h | 2 +- Thing.cpp | 19 ++--- Thing.h | 10 ++- Things/DifferentialDrive.cpp | 25 ++++-- Things/DifferentialDrive.h | 8 +- Things/DigitalSensor.cpp | 12 ++- Things/DigitalSensor.h | 4 +- Things/Motor.cpp | 4 +- Things/RelativeEncoder.cpp | 23 ++++++ Things/RelativeEncoder.h | 38 +++++++++ Things/TemperatureSensor.cpp | 10 ++- Things/TemperatureSensor.h | 3 +- Things/TouchSensor.cpp | 18 +++-- Things/TouchSensor.h | 4 +- 25 files changed, 347 insertions(+), 119 deletions(-) create mode 100644 Things/RelativeEncoder.cpp create mode 100644 Things/RelativeEncoder.h diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 5db7504..80fc402 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -35,10 +35,9 @@ DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing* parent) : RoboidControl::DifferentialDrive(parent), drv8833(config) {} -void DRV8833::DifferentialDrive::Update(unsigned long currentTimeMs, - bool recurse) { - RoboidControl::DifferentialDrive::Update(currentTimeMs, recurse); - this->drv8833.Update(currentTimeMs, recurse); +void DRV8833::DifferentialDrive::Update(bool recurse) { + RoboidControl::DifferentialDrive::Update(recurse); + this->drv8833.Update(recurse); } #pragma endregion Differential drive @@ -80,32 +79,14 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, // this->maxRpm = rpm; // } -// void DRV8833Motor::SetAngularVelocity(Spherical velocity) { void DRV8833Motor::SetTargetSpeed(float motorSpeed) { Motor::SetTargetSpeed(motorSpeed); - // Thing::SetAngularVelocity(velocity); - // ignoring rotation axis for now. - // Spherical angularVelocity = this->GetAngularVelocity(); - // float angularSpeed = velocity.distance; // in degrees/sec - - // float rpm = angularSpeed / 360 * 60; - // float motorSpeed = rpm / this->maxRpm; uint8_t motorSignal = (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255); - // // if (direction == RotationDirection::CounterClockwise) - // // speed = -speed; - // // Determine the rotation direction - // if (velocity.direction.horizontal.InDegrees() < 0) - // motorSpeed = -motorSpeed; - // if (this->reverse) - // motorSpeed = -motorSpeed; - // std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm - // " - // << rpm << ", motor signal = " << (int)motorSignal << "\n"; - std::cout << "moto speed " << this->name << " = " << motorSpeed - << ", motor signal = " << (int)motorSignal << "\n"; + // std::cout << "moto speed " << this->name << " = " << motorSpeed + // << ", motor signal = " << (int)motorSignal << "\n"; #if (ESP32) if (motorSpeed == 0) { // stop diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 838bb9a..2da2664 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -42,8 +42,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { Participant* participant = nullptr); DifferentialDrive(DRV8833::Configuration config, Thing* parent); - virtual void Update(unsigned long currentTimeMs, - bool recurse = false) override; + virtual void Update(bool recurse = false) override; protected: DRV8833 drv8833; @@ -56,7 +55,6 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { /// @brief Support for a DRV8833 motor controller class DRV8833Motor : public Motor { public: - /// @brief Setup the DC motor /// @param pinIn1 the pin number for the in1 signal /// @param pinIn2 the pin number for the in2 signal @@ -65,16 +63,16 @@ class DRV8833Motor : public Motor { unsigned char pinIn1, unsigned char pinIn2, bool reverse = false); - //void SetMaxRPM(unsigned int rpm); + // void SetMaxRPM(unsigned int rpm); - //virtual void SetAngularVelocity(Spherical velocity) override; - virtual void SetTargetSpeed(float targetSpeed) override; - //bool reverse = false; + // virtual void SetAngularVelocity(Spherical velocity) override; + virtual void SetTargetSpeed(float targetSpeed) override; + // bool reverse = false; protected: unsigned char pinIn1 = 255; unsigned char pinIn2 = 255; - //unsigned int maxRpm = 200; + // unsigned int maxRpm = 200; #if (ESP32) uint8_t in1Ch; diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp index d972ddb..5c3a2c2 100644 --- a/Arduino/Things/DigitalInput.cpp +++ b/Arduino/Things/DigitalInput.cpp @@ -5,19 +5,122 @@ namespace RoboidControl { namespace Arduino { +#pragma region Digital input + DigitalInput::DigitalInput(Participant* participant, unsigned char pin) - : TouchSensor(participant) { + : Thing(participant) { this->pin = pin; - pinMode(pin, INPUT); + pinMode(this->pin, INPUT); } -void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) { - this->touchedSomething = digitalRead(pin) == LOW; - // std::cout << "DigitalINput pin " << (int)this->pin << ": " << - // this->touchedSomething << "\n"; - Thing::Update(currentTimeMs, recursive); +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); +} + +#pragma endregion Digital input + +#pragma region Touch sensor + +DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent) + : RoboidControl::TouchSensor(parent), digitalInput(parent, pin) {} + +void DigitalInput::TouchSensor::Update(bool recursive) { + this->touchedSomething = digitalInput.isLow; +} + +#pragma endregion Touch sensor + +#pragma region Relative encoder + +int DigitalInput::RelativeEncoder::interruptCount = 0; +volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0; +volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0; + +DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config, + Thing* parent) + : RoboidControl::RelativeEncoder(parent), + digitalInput(parent, config.pin), + pulsesPerRevolution(config.pulsesPerRevolution) { + + // We support up to 2 pulse counters + if (interruptCount == 0) + attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt0, RISING); + else if (interruptCount == 1) + attachInterrupt(digitalPinToInterrupt(config.pin), PulseInterrupt1, RISING); + else { + // maximum interrupt count reached + std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached" + << std::endl; + return; + } + interruptIx = interruptCount; + interruptCount++; +} + +int DigitalInput::RelativeEncoder::GetPulseCount() { + if (interruptIx == 0) { + int pulseCount = pulseCount0; + pulseCount0 = 0; + return pulseCount; + } else if (interruptIx == 1) { + int pulseCount = pulseCount1; + pulseCount1 = 0; + return pulseCount; + } + return 0; +} + +void DigitalInput::RelativeEncoder::Update(bool recursive) { + unsigned long currentTimeMs = GetTimeMs(); + if (this->lastUpdateTime == 0) { + this->lastUpdateTime = currentTimeMs; + return; + } + + float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f; + int pulseCount = GetPulseCount(); + netPulseCount += pulseCount; + + this->pulseFrequency = pulseCount / timeStep; + this->rotationSpeed = pulseFrequency / pulsesPerRevolution; + + // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency + // << " timestep " << timeStep << std::endl; + + this->lastUpdateTime = currentTimeMs; +} + +#if defined(ESP8266) || defined(ESP32) +void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() { + pulseCount0++; +} +#else +void DigitalInput::RelativeEncoder::PulseInterrupt0() { + pulseCount0++; +} +#endif + +#if defined(ESP8266) || defined(ESP32) +void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() { + pulseCount1++; +} +#else +void DigitalInput::RelativeEncoder::PulseInterrupt1() { + pulseCount1++; +} +#endif + +#pragma endregion Relative encoder + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h index 6099918..966cfa8 100644 --- a/Arduino/Things/DigitalInput.h +++ b/Arduino/Things/DigitalInput.h @@ -1,26 +1,88 @@ #pragma once +#include "Things/RelativeEncoder.h" #include "Things/TouchSensor.h" namespace RoboidControl { namespace Arduino { /// @brief A digital input represents the stat of a digital GPIO pin -class DigitalInput : public TouchSensor { +class DigitalInput : public Thing { public: /// @brief Create a new digital input /// @param participant The participant to use /// @param pin The digital pin DigitalInput(Participant* participant, unsigned char pin); + DigitalInput(Thing* parent, unsigned char pin); + + bool isHigh = false; + bool isLow = false; /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: /// @brief The pin used for digital input unsigned char pin = 0; + + public: + class TouchSensor; + class RelativeEncoder; }; +#pragma region Touch sensor + +class DigitalInput::TouchSensor : public RoboidControl::TouchSensor { + public: + TouchSensor(unsigned char pin, Thing* parent); + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(bool recurse = false) override; + + protected: + DigitalInput digitalInput; +}; + +#pragma endregion Touch sensor + +#pragma region Incremental encoder + +class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder { + public: + struct Configuration { + unsigned char pin; + unsigned char pulsesPerRevolution; + }; + + RelativeEncoder(Configuration config, Thing* parent); + + unsigned char pulsesPerRevolution; + + /// @brief The current pulse frequency in Hz + float pulseFrequency = 0; + + /// @copydoc RoboidControl::Thing::Update() + virtual void Update(bool recurse = false) override; + + protected: + DigitalInput digitalInput; + + int interruptIx = 0; + + static int interruptCount; + + volatile static int pulseCount0; + static void PulseInterrupt0(); + + volatile static int pulseCount1; + static void PulseInterrupt1(); + + int GetPulseCount(); + long netPulseCount = 0; + unsigned long lastUpdateTime = 0; +}; + +#pragma endregion Incremental encoder + } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index f86a6d1..95d0bb6 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -31,7 +31,7 @@ float UltrasonicSensor::GetDistance() { // Measure the duration of the pulse on the echo pin unsigned long duration_us = - pulseIn(pinEcho, HIGH); // the result is in microseconds + pulseIn(pinEcho, HIGH, 10000); // the result is in microseconds // Calculate the distance: // * Duration should be divided by 2, because the ping goes to the object @@ -56,9 +56,9 @@ float UltrasonicSensor::GetDistance() { return this->distance; } -void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) { +void UltrasonicSensor::Update(bool recursive) { GetDistance(); - Thing::Update(currentTimeMs, recursive); + Thing::Update(recursive); } #pragma region Touch sensor @@ -76,9 +76,8 @@ UltrasonicSensor::TouchSensor::TouchSensor( this->touchedSomething = false; } -void UltrasonicSensor::TouchSensor::Update(unsigned long currentTimeMs, - bool recursive) { - this->ultrasonic.Update(currentTimeMs, recursive); +void UltrasonicSensor::TouchSensor::Update(bool recursive) { + this->ultrasonic.Update(recursive); this->touchedSomething = (this->ultrasonic.distance > 0 && this->ultrasonic.distance <= this->touchDistance); } diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index d612777..563594e 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -35,8 +35,7 @@ class UltrasonicSensor : Thing { float GetDistance(); /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: /// @brief The pin number of the trigger signal @@ -58,8 +57,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { float touchDistance = 0.2f; /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) - virtual void Update(unsigned long currentTimeMs, - bool recursive = false) override; + virtual void Update(bool recursive = false) override; protected: UltrasonicSensor ultrasonic; diff --git a/Participant.cpp b/Participant.cpp index 82a3555..03197e6 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -30,10 +30,10 @@ Participant::~Participant() { delete[] this->ipAddress; } -void Participant::Update(unsigned long currentTimeMs) { +void Participant::Update() { for (Thing* thing : this->things) { if (thing != nullptr) - thing->Update(currentTimeMs, true); + thing->Update(true); } } diff --git a/Participant.h b/Participant.h index 358746b..49ee749 100644 --- a/Participant.h +++ b/Participant.h @@ -94,7 +94,7 @@ class Participant { /// @brief Update all things for this participant /// @param currentTimeMs The current time in milliseconds (optional) - virtual void Update(unsigned long currentTimeMs = 0); + virtual void Update(); public: static ParticipantRegistry registry; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index c6c4bb1..5aa4142 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -4,8 +4,8 @@ #include "Arduino/ArduinoParticipant.h" #include "EspIdf/EspIdfParticipant.h" -#include "Windows/WindowsParticipant.h" #include "Posix/PosixParticipant.h" +#include "Windows/WindowsParticipant.h" #include @@ -66,9 +66,8 @@ void ParticipantUDP::SetupUDP(int localPort, #pragma region Update -void ParticipantUDP::Update(unsigned long currentTimeMs) { - if (currentTimeMs == 0) - currentTimeMs = Thing::GetTimeMs(); +void ParticipantUDP::Update() { + unsigned long currentTimeMs = Thing::GetTimeMs(); if (this->isIsolated == false) { if (this->connected == false) @@ -88,11 +87,11 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { this->ReceiveUDP(); } - UpdateMyThings(currentTimeMs); - UpdateOtherThings(currentTimeMs); + UpdateMyThings(); + UpdateOtherThings(); } -void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { +void ParticipantUDP::UpdateMyThings() { for (Thing* thing : this->things) { if (thing == nullptr) // || thing->GetParent() != nullptr) continue; @@ -102,7 +101,7 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Send(this->remoteSite, thingMsg); delete thingMsg; - + if (thing->nameChanged) { NameMsg* nameMsg = new NameMsg(this->networkId, thing); this->Send(this->remoteSite, nameMsg); @@ -115,7 +114,7 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { // 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(currentTimeMs, false); + thing->Update(false); if (!(this->isIsolated || this->networkId == 0)) { if (thing->terminate) { @@ -142,21 +141,21 @@ void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { } } -void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { +void ParticipantUDP::UpdateOtherThings() { #if defined(NO_STD) Participant** participants = Participant::registry.GetAll(); for (int ix = 0; ix < Participant::registry.count; ix++) { Participant* participant = participants[ix]; -#else +#else for (Participant* participant : Participant::registry.GetAll()) { -#endif +#endif if (participant == nullptr || participant == this) continue; // Call only the Participant version of the Update. // This is to deal with the function where one of the (remote) // participants is actually a local participant - participant->Participant::Update(currentTimeMs); + participant->Participant::Update(); if (this->isIsolated) continue; diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 8c2efdf..c51131d 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -100,13 +100,13 @@ class ParticipantUDP : public Participant { #pragma region Update public: - virtual void Update(unsigned long currentTimeMs = 0) override; + virtual void Update() override; protected: unsigned long nextPublishMe = 0; - virtual void UpdateMyThings(unsigned long currentTimeMs); - virtual void UpdateOtherThings(unsigned long currentTimeMs); + virtual void UpdateMyThings(); + virtual void UpdateOtherThings(); #pragma endregion Update diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 5dc6361..a24bf09 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -22,12 +22,12 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) { #pragma region Update -void SiteServer::UpdateMyThings(unsigned long currentTimeMs) { +void SiteServer::UpdateMyThings() { for (Thing* thing : this->things) { if (thing == nullptr) continue; - thing->Update(currentTimeMs, true); + thing->Update( true); if (this->isIsolated == false) { // Send to all other participants diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index a4e6767..67bc5bf 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -24,7 +24,7 @@ class SiteServer : public ParticipantUDP { #pragma region Update - virtual void UpdateMyThings(unsigned long currentTimeMs) override; + virtual void UpdateMyThings() override; #pragma endregion Update diff --git a/Thing.cpp b/Thing.cpp index e14a2bc..bbf82e3 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -230,7 +230,8 @@ Spherical Thing::GetAngularVelocity() { unsigned long Thing::GetTimeMs() { #if defined(ARDUINO) - return millis(); + unsigned long ms = millis(); + return ms; #else auto now = std::chrono::steady_clock::now(); auto ms = std::chrono::duration_cast( @@ -239,11 +240,11 @@ unsigned long Thing::GetTimeMs() { #endif } -void Thing::Update(bool recursive) { - Update(GetTimeMs(), recursive); -} +// void Thing::Update(bool recursive) { +// Update(GetTimeMs(), recursive); +// } -void Thing::Update(unsigned long currentTimeMs, bool recursive) { +void Thing::Update(bool recursive) { // if (this->positionUpdated || this->orientationUpdated) // OnPoseChanged callback this->positionUpdated = false; @@ -254,18 +255,18 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { this->nameChanged = false; if (recursive) { - // std::cout << "# chilren: " << (int)this->childCount << std::endl; + // std::cout << "# children: " << (int)this->childCount << std::endl; for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx]; if (child == nullptr) continue; - child->Update(currentTimeMs, recursive); + child->Update(recursive); } } } -void Thing::UpdateThings(unsigned long currentTimeMs) { - IsolatedParticipant::Isolated()->Update(currentTimeMs); +void Thing::UpdateThings() { + IsolatedParticipant::Isolated()->Update(); } #pragma endregion Update diff --git a/Thing.h b/Thing.h index 0827886..9045430 100644 --- a/Thing.h +++ b/Thing.h @@ -32,6 +32,7 @@ class Thing { ControlledMotor, UncontrolledMotor, Servo, + IncrementalEncoder, // Other Roboid, Humanoid, @@ -63,7 +64,8 @@ class Thing { Thing(Thing* parent, unsigned char thingType = 0); //, unsigned char thingId = 0); static Thing Reconstruct(Participant* owner, unsigned char thingType, unsigned char thingId); -#pragma endregion Init + + #pragma endregion Init public: /// @brief Terminated things are no longer updated @@ -214,15 +216,15 @@ class Thing { static unsigned long GetTimeMs(); /// @brief Updates the state of the thing - void Update(bool recursive = false); + //void Update(bool recursive = false); /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds; if this is /// zero, the current time is retrieved automatically /// @param recurse When true, this will Update the descendants recursively - virtual void Update(unsigned long currentTimeMs, bool recurse = false); + virtual void Update(bool recurse = false); - static void UpdateThings(unsigned long currentTimeMs); + static void UpdateThings(); #pragma endregion Update diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index f8fc0ca..30b3c0b 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -2,15 +2,24 @@ namespace RoboidControl { -DifferentialDrive::DifferentialDrive(Participant* participant, - unsigned char thingId) - : Thing(participant, Thing::Type::DifferentialDrive, thingId) { +// 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, unsigned char thingId) - : Thing(parent, Thing::Type::DifferentialDrive, thingId) {} void DifferentialDrive::SetDriveDimensions(float wheelDiameter, float wheelSeparation) { @@ -47,7 +56,7 @@ void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) { Spherical(speedRight, Direction::right)); } -void DifferentialDrive::Update(unsigned long currentMs, bool recursive) { +void DifferentialDrive::Update(bool recursive) { if (this->linearVelocityUpdated) { // this assumes forward velocity only.... float linearVelocity = this->GetLinearVelocity().distance; @@ -68,7 +77,7 @@ void DifferentialDrive::Update(unsigned long currentMs, bool recursive) { this->SetWheelVelocity(speedLeft, speedRight); } - Thing::Update(currentMs, recursive); + Thing::Update(recursive); } } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index f352611..2647a12 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -16,13 +16,13 @@ class DifferentialDrive : public Thing { /// @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 = nullptr, - unsigned char thingId = 0); + DifferentialDrive(Participant* participant = nullptr); //, + //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); //, unsigned char thingId = 0); /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -45,7 +45,7 @@ class DifferentialDrive : public Thing { void SetWheelVelocity(float speedLeft, float speedRight); /// @copydoc RoboidControl::Thing::Update(unsigned long) - virtual void Update(unsigned long currentMs, bool recursive = true) override; + virtual void Update(bool recursive = true) override; /// @brief The left wheel Motor* leftWheel = nullptr; diff --git a/Things/DigitalSensor.cpp b/Things/DigitalSensor.cpp index 5eae131..08a959e 100644 --- a/Things/DigitalSensor.cpp +++ b/Things/DigitalSensor.cpp @@ -4,11 +4,15 @@ namespace RoboidControl { //DigitalSensor::DigitalSensor() : Thing(Type::Switch) {} -DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId) - : Thing(owner, Type::Switch, thingId) {} +// DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId) +// : Thing(owner, Type::Switch, thingId) {} -DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) - : Thing(parent, Type::Switch) {} +// DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId) +// : Thing(parent, Type::Switch) {} + +DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {} + +DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {} int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = state ? 1 : 0; diff --git a/Things/DigitalSensor.h b/Things/DigitalSensor.h index 520ae4c..a9c0571 100644 --- a/Things/DigitalSensor.h +++ b/Things/DigitalSensor.h @@ -13,12 +13,12 @@ class DigitalSensor : public Thing { /// @param owner The owning participant /// @param thingId The ID of the thing, leave out or set to zero to generate /// an ID - DigitalSensor(Participant* owner = nullptr, unsigned char thingId = 0); + DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0); /// @brief Create a new child digital sensor /// @param parent The parent 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); //, unsigned char thingId = 0); /// @brief The sigital state bool state = 0; diff --git a/Things/Motor.cpp b/Things/Motor.cpp index 7d3abd7..96b50a6 100644 --- a/Things/Motor.cpp +++ b/Things/Motor.cpp @@ -1,8 +1,8 @@ #include "Motor.h" -RoboidControl::Motor::Motor(Participant* owner) {} +RoboidControl::Motor::Motor(Participant* owner) : Thing(owner, Type::UncontrolledMotor) {} -RoboidControl::Motor::Motor(Thing* parent) {} +RoboidControl::Motor::Motor(Thing* parent) : Thing(parent, Type::UncontrolledMotor) {} void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) { this->targetSpeed = targetSpeed; diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp new file mode 100644 index 0000000..dd10cdd --- /dev/null +++ b/Things/RelativeEncoder.cpp @@ -0,0 +1,23 @@ +#include "RelativeEncoder.h" + +namespace RoboidControl { + +RelativeEncoder::RelativeEncoder(Participant* owner) + : Thing(owner, Type::IncrementalEncoder) {} +RelativeEncoder::RelativeEncoder(Thing* parent) + : Thing(parent, Type::IncrementalEncoder) {} + +float RelativeEncoder::GetRotationSpeed() { + return rotationSpeed; +} + +int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) { + data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127); + return 1; +} + +void RelativeEncoder::ProcessBinary(char* data) { + this->rotationSpeed = (float)data[0] / 127; +} + +} // namespace RoboidControl diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h new file mode 100644 index 0000000..fec8c2b --- /dev/null +++ b/Things/RelativeEncoder.h @@ -0,0 +1,38 @@ +#pragma once + +#include "Thing.h" + +namespace RoboidControl { + +/// @brief An Incremental Encoder measures the rotations of an axle using a +/// rotary sensor. Some encoders are able to detect direction, while others can +/// not. +class RelativeEncoder : public Thing { + public: + /// @brief Creates a sensor which measures distance from pulses + /// @param pulsesPerRevolution The number of pulse edges which make a + /// full rotation + /// @param distancePerRevolution The distance a wheel travels per full + /// rotation + RelativeEncoder(Participant* owner); + RelativeEncoder(Thing* parent); + + /// @brief Get the rotation speed + /// @return The speed in revolutions per second + virtual float GetRotationSpeed(); + float rotationSpeed; + + /// @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 + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @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: + /// @brief rotation speed in revolutions per second + //float _rotationSpeed; +}; + +} // namespace RoboidControl diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index 45718a5..632b3c0 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -4,9 +4,13 @@ namespace RoboidControl { -TemperatureSensor::TemperatureSensor(Participant* participant, - unsigned char thingId) - : Thing(participant, Type::TemperatureSensor, thingId) {} +// TemperatureSensor::TemperatureSensor(Participant* participant, +// unsigned char thingId) +// : Thing(participant, Type::TemperatureSensor, thingId) {} + +TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {} + +TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {} void TemperatureSensor::SetTemperature(float temp) { this->temperature = temp; diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index a447385..629c178 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -12,7 +12,8 @@ class TemperatureSensor : public Thing { /// @brief Create a temperature sensor with the given ID /// @param networkId The network ID of the sensor /// @param thingId The ID of the thing - TemperatureSensor(Participant* participant, unsigned char thingId); + TemperatureSensor(Participant* participant); //, unsigned char thingId); + TemperatureSensor(Thing* parent); /// @brief The measured temperature float temperature = 0; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 809fa9d..7657072 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -2,14 +2,20 @@ namespace RoboidControl { -//TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} +// TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {} -TouchSensor::TouchSensor(Participant* owner, unsigned char thingId) - : Thing(owner, Thing::Type::TouchSensor, thingId) {} +// 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(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) {} int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { bytes[(*ix)++] = touchedSomething ? 1 : 0; diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index c176145..e857f90 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -14,12 +14,12 @@ class TouchSensor : public Thing { /// @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 = nullptr, unsigned char thingId = 0); + TouchSensor(Participant* owner = nullptr); //, 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); //, unsigned char thingId = 0); /// @brief Value which is true when the sensor is touching something, false /// otherwise