From d22fc8e244290b0141a7afe59204e631814ab4d6 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 29 Jan 2024 10:46:36 +0100 Subject: [PATCH 1/4] Improved encoder support --- AbsoluteEncoder.cpp | 2 ++ FeedbackServo.h => AbsoluteEncoder.h | 5 +++-- Encoder.cpp | 19 +++++++++++-------- Encoder.h | 20 +++++++++++++------- FeedbackServo.cpp | 3 --- IncrementalEncoder.cpp | 19 +++++++++++++++++++ 6 files changed, 48 insertions(+), 20 deletions(-) create mode 100644 AbsoluteEncoder.cpp rename FeedbackServo.h => AbsoluteEncoder.h (67%) delete mode 100644 FeedbackServo.cpp create mode 100644 IncrementalEncoder.cpp diff --git a/AbsoluteEncoder.cpp b/AbsoluteEncoder.cpp new file mode 100644 index 0000000..6606485 --- /dev/null +++ b/AbsoluteEncoder.cpp @@ -0,0 +1,2 @@ +#include "AbsoluteEncoder.h" + diff --git a/FeedbackServo.h b/AbsoluteEncoder.h similarity index 67% rename from FeedbackServo.h rename to AbsoluteEncoder.h index 5201b9d..bcc643e 100644 --- a/FeedbackServo.h +++ b/AbsoluteEncoder.h @@ -5,11 +5,12 @@ namespace Passer { namespace RoboidContol { -class FeedbackServo : public ServoMotor { +class AbsoluteEncoder { public: - FeedbackServo(); + AbsoluteEncoder() {} virtual float GetActualAngle() = 0; + virtual float GetActualVelocity() = 0; }; } // namespace RoboidContol diff --git a/Encoder.cpp b/Encoder.cpp index 07e2558..f9a472b 100644 --- a/Encoder.cpp +++ b/Encoder.cpp @@ -1,18 +1,21 @@ +/* #include "Encoder.h" -Encoder::Encoder(unsigned char pulsesPerRevolution, - unsigned char distancePerRotation) { - //: Encoder::Encoder() { +IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution, + unsigned char distancePerRotation) { this->pulsesPerRevolution = pulsesPerRevolution; this->distancePerRevolution = distancePerRotation; } -int Encoder::GetPulseCount() { return 0; } +int IncrementalEncoder::GetPulseCount() { return 0; } -float Encoder::GetDistance() { return 0; } +float IncrementalEncoder::GetDistance() { return 0; } -float Encoder::GetPulsesPerSecond(float currentTimeMs) { return 0; } +float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; } -float Encoder::GetRevolutionsPerSecond(float currentTimeMs) { return 0; } +float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) { + return 0; +} -float Encoder::GetSpeed(float currentTimeMs) { return 0; } +float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; } +*/ \ No newline at end of file diff --git a/Encoder.h b/Encoder.h index e85cdaf..bf0a672 100644 --- a/Encoder.h +++ b/Encoder.h @@ -1,19 +1,25 @@ #pragma once +#include "IncrementalEncoder.h" + namespace Passer { namespace RoboidControl { -/// @brief An Encoder measures the rotations of an axle using a rotary sensor -/// Some encoders are able to detect direction, while others can not. -class Encoder { +// Deprecated, use explicit IncrementalEncoder in the future +using Encoder = IncrementalEncoder; + +/* +/// @brief An Encoder measures the rotations of an axle using a rotary +/// sensor Some encoders are able to detect direction, while others can not. +class IncrementalEncoder { 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 - Encoder(unsigned char pulsesPerRevolution = 1, - unsigned char distancePerRevolution = 1); + IncrementalEncoder(unsigned char pulsesPerRevolution = 1, + unsigned char distancePerRevolution = 1); /// @brief Get the total number of pulses since the previous call /// @return The number of pulses, is zero or greater @@ -45,7 +51,7 @@ public: /// meter unsigned char distancePerRevolution = 1; }; - +*/ } // namespace RoboidControl } // namespace Passer -using namespace Passer::RoboidControl; \ No newline at end of file +using namespace Passer::RoboidControl; diff --git a/FeedbackServo.cpp b/FeedbackServo.cpp deleted file mode 100644 index cab0e95..0000000 --- a/FeedbackServo.cpp +++ /dev/null @@ -1,3 +0,0 @@ -#include "FeedbackServo.h" - -FeedbackServo::FeedbackServo() {} \ No newline at end of file diff --git a/IncrementalEncoder.cpp b/IncrementalEncoder.cpp new file mode 100644 index 0000000..2da89b8 --- /dev/null +++ b/IncrementalEncoder.cpp @@ -0,0 +1,19 @@ +#include "Encoder.h" + +IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution, + unsigned char distancePerRotation) { + this->pulsesPerRevolution = pulsesPerRevolution; + this->distancePerRevolution = distancePerRotation; +} + +int IncrementalEncoder::GetPulseCount() { return 0; } + +float IncrementalEncoder::GetDistance() { return 0; } + +float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; } + +float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) { + return 0; +} + +float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; } From ae629745623bdb1ecbaa7839eb6177cbcca417ec Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 29 Jan 2024 10:47:00 +0100 Subject: [PATCH 2/4] Generic velocity control for servos --- ServoMotor.cpp | 76 +++++++++++++++++++++++++++++++++++++++++++++++++- ServoMotor.h | 29 ++++++++++++++++++- 2 files changed, 103 insertions(+), 2 deletions(-) diff --git a/ServoMotor.cpp b/ServoMotor.cpp index c0a7781..7978534 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -1,3 +1,77 @@ #include "ServoMotor.h" -ServoMotor::ServoMotor() {} \ No newline at end of file +#include "VectorAlgebra/FloatSingle.h" + +ServoMotor::ServoMotor() { this->controlMode = ControlMode::Position; } + +void ServoMotor::SetTargetAngle(float targetAngle) { + targetAngle = Float::Clamp(targetAngle, minAngle, maxAngle); + + if (maxVelocity == 0.0F) { + SetAngle(targetAngle); + } else if (targetAngle != this->targetAngle) { + // if the new target is the same, the limited angle is not overwritten + this->limitedTargetAngle = this->targetAngle; + } + + this->controlMode = ControlMode::Position; + this->targetAngle = targetAngle; +} + +float ServoMotor::GetTargetAngle() { return this->targetAngle; } + +void ServoMotor::SetMaximumVelocity(float maxVelocity) { + this->maxVelocity = maxVelocity; +} + +void ServoMotor::SetTargetVelocity(float velocity) { + targetVelocity = + Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity); + + this->controlMode = ControlMode::Velocity; + this->targetVelocity = targetVelocity; + this->hasTargetAngle = false; // can't we use the controlMode for this? +} + +float ServoMotor::GetTargetVelocity() { return this->targetVelocity; } + +void ServoMotor::Update(float currentTimeMs) { + if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) { + this->lastUpdateTimeMs = currentTimeMs; + return; + } + + float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F; + + if (controlMode == ControlMode::Position) { + if (maxVelocity == 0.0F || hasTargetAngle == false) { + this->lastUpdateTimeMs = currentTimeMs; + return; + + } else { + float angleStep = maxVelocity * deltaTime; + float deltaAngle = this->targetAngle - this->limitedTargetAngle; + if (deltaAngle < 0) + deltaAngle = -deltaAngle; + + if (deltaAngle < angleStep) { + this->limitedTargetAngle = this->targetAngle; + SetAngle(targetAngle); + } else { + limitedTargetAngle += angleStep; + } + SetAngle(limitedTargetAngle); + + this->lastUpdateTimeMs = currentTimeMs; + return; + } + } + + float newAngle = this->targetAngle + targetVelocity * deltaTime; + newAngle = Float::Clamp(newAngle, minAngle, maxAngle); + + ServoMotor::SetTargetAngle(newAngle); + SetAngle(newAngle); + + this->lastUpdateTimeMs = currentTimeMs; +} \ No newline at end of file diff --git a/ServoMotor.h b/ServoMotor.h index 748d502..a32583e 100644 --- a/ServoMotor.h +++ b/ServoMotor.h @@ -9,7 +9,34 @@ class ServoMotor : public Thing { public: ServoMotor(); - virtual void SetTargetAngle(float angle) = 0; + float minAngle = -90.0F; + float maxAngle = 90.0F; + + enum ControlMode { Position, Velocity }; + ControlMode controlMode = ControlMode::Position; + + virtual void SetTargetAngle(float angle); + virtual float GetTargetAngle(); + + virtual void SetMaximumVelocity(float maxVelocity); + + virtual void SetTargetVelocity(float velocity); + virtual float GetTargetVelocity(); + + virtual void Update(float currentTimeMs); + +protected: + bool hasTargetAngle = false; + float targetAngle = 0.0F; + + float maxVelocity = 0.0F; + + float targetVelocity = 0.0F; + float limitedTargetAngle = 0.0F; + + float lastUpdateTimeMs = 0.0F; + + virtual void SetAngle(float angle) = 0; }; } // namespace RoboidContol From e995d4cec9a2c7dd630edd7868592c9e434a733c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 29 Jan 2024 10:48:37 +0100 Subject: [PATCH 3/4] Fixed missing IncrementalEncoder.h --- IncrementalEncoder.h | 51 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 IncrementalEncoder.h diff --git a/IncrementalEncoder.h b/IncrementalEncoder.h new file mode 100644 index 0000000..83bd135 --- /dev/null +++ b/IncrementalEncoder.h @@ -0,0 +1,51 @@ +#pragma once + +namespace Passer { +namespace RoboidControl { + +/// @brief An Encoder measures the rotations of an axle using a rotary +/// sensor Some encoders are able to detect direction, while others can not. +class IncrementalEncoder { +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 + IncrementalEncoder(unsigned char pulsesPerRevolution = 1, + unsigned char distancePerRevolution = 1); + + /// @brief Get the total number of pulses since the previous call + /// @return The number of pulses, is zero or greater + virtual int GetPulseCount(); + /// @brief Get the pulse speed since the previous call + /// @param currentTimeMs The clock time in milliseconds + /// @return The average pulses per second in the last period. + virtual float GetPulsesPerSecond(float currentTimeMs); + + /// @brief Get the distance traveled since the previous call + /// @return The distance in meters. + virtual float GetDistance(); + + /// @brief Get the rotation speed since the previous call + /// @param currentTimeMs The clock time in milliseconds + /// @return The speed in rotations per second + virtual float GetRevolutionsPerSecond(float currentTimeMs); + + /// @brief Get the speed since the previous call + /// @param currentTimeMs The clock time in milliseconds + /// @return The speed in meters per second. + /// @note this value is dependent on the accurate setting of the + /// pulsesPerRevolution and distancePerRevolution parameters; + virtual float GetSpeed(float currentTimeMs); + + /// @brief The numer of pulses corresponding to a full rotation of the axle + unsigned char pulsesPerRevolution = 1; + /// @brief The number of revolutions which makes the wheel move forward 1 + /// meter + unsigned char distancePerRevolution = 1; +}; + +} // namespace RoboidControl +} // namespace Passer +using namespace Passer::RoboidControl; \ No newline at end of file From af3d2531b2afcd2148dedb551da846aaaaaa3da0 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 29 Jan 2024 11:58:23 +0100 Subject: [PATCH 4/4] Fixed issues --- ServoMotor.cpp | 49 +++++++++++++++++++++++++++++-------------------- 1 file changed, 29 insertions(+), 20 deletions(-) diff --git a/ServoMotor.cpp b/ServoMotor.cpp index 7978534..633844c 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -2,20 +2,26 @@ #include "VectorAlgebra/FloatSingle.h" -ServoMotor::ServoMotor() { this->controlMode = ControlMode::Position; } +ServoMotor::ServoMotor() { + this->controlMode = ControlMode::Position; + this->targetAngle = 0; + this->hasTargetAngle = false; +} -void ServoMotor::SetTargetAngle(float targetAngle) { - targetAngle = Float::Clamp(targetAngle, minAngle, maxAngle); +void ServoMotor::SetTargetAngle(float angle) { + angle = Float::Clamp(angle, minAngle, maxAngle); - if (maxVelocity == 0.0F) { - SetAngle(targetAngle); - } else if (targetAngle != this->targetAngle) { + if (maxVelocity == 0.0F || hasTargetAngle == false) { + SetAngle(angle); + this->limitedTargetAngle = angle; + } else if (angle != this->targetAngle) { // if the new target is the same, the limited angle is not overwritten this->limitedTargetAngle = this->targetAngle; } this->controlMode = ControlMode::Position; - this->targetAngle = targetAngle; + this->targetAngle = angle; + this->hasTargetAngle = true; } float ServoMotor::GetTargetAngle() { return this->targetAngle; } @@ -24,7 +30,7 @@ void ServoMotor::SetMaximumVelocity(float maxVelocity) { this->maxVelocity = maxVelocity; } -void ServoMotor::SetTargetVelocity(float velocity) { +void ServoMotor::SetTargetVelocity(float targetVelocity) { targetVelocity = Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity); @@ -51,27 +57,30 @@ void ServoMotor::Update(float currentTimeMs) { } else { float angleStep = maxVelocity * deltaTime; float deltaAngle = this->targetAngle - this->limitedTargetAngle; - if (deltaAngle < 0) - deltaAngle = -deltaAngle; + float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle; - if (deltaAngle < angleStep) { + if (absDeltaAngle < angleStep) { this->limitedTargetAngle = this->targetAngle; SetAngle(targetAngle); } else { - limitedTargetAngle += angleStep; + if (deltaAngle < 0) + limitedTargetAngle -= angleStep; + else + limitedTargetAngle += angleStep; } SetAngle(limitedTargetAngle); this->lastUpdateTimeMs = currentTimeMs; return; } + + } else { // Velocity Control + float newAngle = this->targetAngle + targetVelocity * deltaTime; + newAngle = Float::Clamp(newAngle, minAngle, maxAngle); + + ServoMotor::SetTargetAngle(newAngle); + SetAngle(newAngle); + + this->lastUpdateTimeMs = currentTimeMs; } - - float newAngle = this->targetAngle + targetVelocity * deltaTime; - newAngle = Float::Clamp(newAngle, minAngle, maxAngle); - - ServoMotor::SetTargetAngle(newAngle); - SetAngle(newAngle); - - this->lastUpdateTimeMs = currentTimeMs; } \ No newline at end of file