From dbe9c9237f7a52dab0907bac4e4e1e41c3dd0f2b Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 8 Apr 2025 10:49:31 +0200 Subject: [PATCH] Initial merge from haptic arm --- Arduino/ArduinoParticipant.h | 2 +- EspIdf/EspIdfParticipant.h | 4 +- Things/AbsoluteEncoder.cpp | 2 + Things/AbsoluteEncoder.h | 15 +++++ Things/ControlledMotor.cpp | 63 ++++++++++++++++++++ Things/ControlledMotor.h | 57 ++++++++++++++++++ Things/IncrementalEncoder.cpp | 19 ++++++ Things/IncrementalEncoder.h | 48 ++++++++++++++++ Things/Motor.cpp | 21 +++++++ Things/Motor.h | 42 ++++++++++++++ Things/ServoMotor.cpp | 105 ++++++++++++++++++++++++++++++++++ Things/ServoMotor.h | 47 +++++++++++++++ 12 files changed, 422 insertions(+), 3 deletions(-) create mode 100644 Things/AbsoluteEncoder.cpp create mode 100644 Things/AbsoluteEncoder.h create mode 100644 Things/ControlledMotor.cpp create mode 100644 Things/ControlledMotor.h create mode 100644 Things/IncrementalEncoder.cpp create mode 100644 Things/IncrementalEncoder.h create mode 100644 Things/Motor.cpp create mode 100644 Things/Motor.h create mode 100644 Things/ServoMotor.cpp create mode 100644 Things/ServoMotor.h diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 8887ade..1cf39a0 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -1,6 +1,6 @@ #pragma once -#include "../LocalParticipant.h" +#include "Participants/ParticipantUDP.h" #if defined(HAS_WIFI) #include diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index 9a1c549..1ff7a64 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -1,13 +1,13 @@ #pragma once -#include "../LocalParticipant.h" +#include "Participants/ParticipantUDP.h" #include "lwip/sockets.h" namespace RoboidControl { namespace EspIdf { -class LocalParticipant : public RoboidControl::LocalParticipant { +class ParticipantUDP : public RoboidControl::LocalParticipant { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); diff --git a/Things/AbsoluteEncoder.cpp b/Things/AbsoluteEncoder.cpp new file mode 100644 index 0000000..6606485 --- /dev/null +++ b/Things/AbsoluteEncoder.cpp @@ -0,0 +1,2 @@ +#include "AbsoluteEncoder.h" + diff --git a/Things/AbsoluteEncoder.h b/Things/AbsoluteEncoder.h new file mode 100644 index 0000000..2e865d8 --- /dev/null +++ b/Things/AbsoluteEncoder.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ServoMotor.h" + +namespace RoboidControl { + +class AbsoluteEncoder { + public: + AbsoluteEncoder() {} + + virtual Angle16 GetActualAngle() = 0; + virtual float GetActualVelocity() = 0; +}; + +} // namespace RoboidControl diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp new file mode 100644 index 0000000..bec227e --- /dev/null +++ b/Things/ControlledMotor.cpp @@ -0,0 +1,63 @@ +#include "ControlledMotor.h" + +ControlledMotor::ControlledMotor() { this->type = Thing::ControlledMotorType; } + +ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder) + : ControlledMotor() { + this->motor = motor; + this->encoder = encoder; + // this->rotationDirection = Direction::Forward; +} + +#include + +void ControlledMotor::SetTargetSpeed(float speed) { + this->currentTargetSpeed = speed; + // this->rotationDirection = + // (targetSpeed < 0) ? Direction::Reverse : Direction::Forward; + // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise + // : Motor::Direction::Clockwise; +} + +void ControlledMotor::Update(unsigned long currentTimeMs) { + this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs); + if (this->currentTargetSpeed < 0) + this->actualSpeed = -this->actualSpeed; + + float deltaTime = currentTimeMs - this->lastUpdateTime; + + float error = this->currentTargetSpeed - this->actualSpeed; + float errorChange = (error - lastError) * deltaTime; + + float delta = error * pidP + errorChange * pidD; + + Serial.print(" actual Speed "); + Serial.print(actualSpeed); + Serial.print(" target Speed "); + Serial.print(this->currentTargetSpeed); + Serial.print(" motor target speed "); + Serial.print(motor->currentTargetSpeed); + Serial.print(" + "); + Serial.print(error * pidP); + Serial.print(" + "); + Serial.print(errorChange * pidD); + Serial.print(" = "); + Serial.println(motor->currentTargetSpeed + delta); + + motor->SetTargetSpeed(motor->currentTargetSpeed + + delta); // or something like that + this->lastUpdateTime = currentTimeMs; +} + +float ControlledMotor::GetActualSpeed() { return actualSpeed; } + +bool ControlledMotor::Drive(float distance) { + if (!driving) { + targetDistance = distance; + startDistance = encoder->GetDistance(); + driving = true; + } + float totalDistance = encoder->GetDistance() - startDistance; + bool completed = totalDistance > targetDistance; + return completed; +} \ No newline at end of file diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h new file mode 100644 index 0000000..3d9ebda --- /dev/null +++ b/Things/ControlledMotor.h @@ -0,0 +1,57 @@ +#pragma once + +#include "IncrementalEncoder.h" +#include "Motor.h" + +namespace RoboidControl { + +/// @brief A motor with speed control +/// It uses a feedback loop from an encoder to regulate the speed +/// The speed is measured in revolutions per second. +class ControlledMotor : public Motor { + public: + ControlledMotor(); + ControlledMotor(Motor* motor, IncrementalEncoder* encoder); + + inline static bool CheckType(Thing* thing) { + return (thing->type & (int)Thing::Type::ControlledMotor) != 0; + } + float velocity; + + float pidP = 0.1F; + float pidD = 0.0F; + float pidI = 0.0F; + + void Update(unsigned long currentTimeMs) override; + + /// @brief Set the target speed for the motor controller + /// @param speed the target in revolutions per second. + virtual void SetTargetSpeed(float speed) override; + + /// @brief Get the actual speed from the encoder + /// @return The speed in revolutions per second + virtual float GetActualSpeed() override; + + bool Drive(float distance); + + Motor* motor; + IncrementalEncoder* encoder; + + protected: + float lastUpdateTime = 0; + float lastError = 0; + // float targetSpeed; + float actualSpeed; + float netDistance = 0; + float startDistance = 0; + + // enum Direction { Forward = 1, Reverse = -1 }; + + // Direction rotationDirection; + + bool driving = false; + float targetDistance = 0; + float lastEncoderPosition = 0; +}; + +} // namespace RoboidControl diff --git a/Things/IncrementalEncoder.cpp b/Things/IncrementalEncoder.cpp new file mode 100644 index 0000000..2da89b8 --- /dev/null +++ b/Things/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; } diff --git a/Things/IncrementalEncoder.h b/Things/IncrementalEncoder.h new file mode 100644 index 0000000..33a9074 --- /dev/null +++ b/Things/IncrementalEncoder.h @@ -0,0 +1,48 @@ +#pragma once + +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 diff --git a/Things/Motor.cpp b/Things/Motor.cpp new file mode 100644 index 0000000..5b69781 --- /dev/null +++ b/Things/Motor.cpp @@ -0,0 +1,21 @@ +#include "Motor.h" + +#include + +namespace RoboidControl { + +Motor::Motor() : Thing(0) { // for now, id should be set properly later + this->type = (int)Thing::UncontrolledMotor; +} + +float Motor::GetActualSpeed() { return this->currentTargetSpeed; } + +void Motor::SetTargetSpeed(float targetSpeed) { + this->currentTargetSpeed = targetSpeed; +} + +float Motor::GetTargetSpeed() { return (this->currentTargetSpeed); } + +void Motor::Update(unsigned long currentTimeMs) {} + +} \ No newline at end of file diff --git a/Things/Motor.h b/Things/Motor.h new file mode 100644 index 0000000..9893cb3 --- /dev/null +++ b/Things/Motor.h @@ -0,0 +1,42 @@ +#pragma once + +#include "Thing.h" + +#include + +namespace RoboidControl { + +/// @brief A Motor is a Thing which can move parts of the Roboid +/// @note Currently only rotational motors are supported +class Motor : public Thing { +public: + /// @brief Default constructor for the Motor + Motor(); + + /// @brief Motor turning direction + enum class Direction { Clockwise = 1, CounterClockwise = -1 }; + /// @brief The forward turning direction of the motor + Direction direction = Direction::Clockwise; + + /// @brief Set the target motor speed + /// @param speed The speed between -1 (full backward), 0 (stop) and 1 (full + /// forward) + virtual void SetTargetSpeed(float speed); + /// @brief Get the current target speed of the motor + /// @return The speed between -1 (full backward), 0 (stop) and 1 (full + /// forward) + virtual float GetTargetSpeed(); + + /// @brief Get the current actual speed of the motor + /// @return The speed between -1 (full backward), 0 (stop) and 1 (full + /// forward) + virtual float GetActualSpeed(); + + virtual void Update(unsigned long currentTimeMs); + + float currentTargetSpeed = 0; + +protected: +}; + +} // namespace RoboidControl diff --git a/Things/ServoMotor.cpp b/Things/ServoMotor.cpp new file mode 100644 index 0000000..4f99da7 --- /dev/null +++ b/Things/ServoMotor.cpp @@ -0,0 +1,105 @@ +#include "ServoMotor.h" + +#include "LinearAlgebra/FloatSingle.h" + +ServoMotor::ServoMotor() + : Thing(0) { // for now, id should be set properly later + this->type = Thing::ServoType; + this->controlMode = ControlMode::Position; + this->targetAngle = Angle16(); + this->hasTargetAngle = false; +} + +void ServoMotor::SetTargetAngle(Angle16 angle) { + angle = Angle16::Clamp(angle, minAngle, maxAngle); + + if (maxSpeed == 0.0F) { + SetAngle(angle); + this->limitedTargetAngle = angle; + } + + this->controlMode = ControlMode::Position; + this->targetAngle = angle; + this->hasTargetAngle = true; +} + +Angle16 ServoMotor::GetTargetAngle() { + return this->targetAngle; +} + +void ServoMotor::SetMaximumVelocity(float maxVelocity) { + this->maxSpeed = maxVelocity; +} + +void ServoMotor::SetTargetVelocity(float targetVelocity) { + targetVelocity = Float::Clamp(targetVelocity, -this->maxSpeed, maxSpeed); + + 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(unsigned long currentTimeMs) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing* child = this->GetChild(childIx); + if (child != nullptr && child->type == Thing::ServoType) { + ServoMotor* servo = (ServoMotor*)child; + servo->Update(currentTimeMs); + } + } + + if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) { + this->lastUpdateTimeMs = currentTimeMs; + return; + } + + float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F; + + // Position control + if (controlMode == ControlMode::Position) { + if (maxSpeed == 0.0F || hasTargetAngle == false) { + this->lastUpdateTimeMs = currentTimeMs; + return; + + } else { + float angleStep = maxSpeed * deltaTime; + float deltaAngle = + this->targetAngle.InDegrees() - this->limitedTargetAngle.InDegrees(); + float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle; + + if (absDeltaAngle < angleStep) { + this->limitedTargetAngle = this->targetAngle; + SetAngle(targetAngle); + } else { + if (deltaAngle < 0) + limitedTargetAngle = limitedTargetAngle - Angle16::Degrees(angleStep); + else + limitedTargetAngle = limitedTargetAngle + Angle16::Degrees(angleStep); + } + SetAngle(limitedTargetAngle); + + this->lastUpdateTimeMs = currentTimeMs; + return; + } + + // Velocity control + } else { + float newAngle = this->targetAngle.InDegrees() + targetVelocity * deltaTime; + newAngle = + Float::Clamp(newAngle, minAngle.InDegrees(), maxAngle.InDegrees()); + + Angle16 targetAngle = Angle16::Degrees(newAngle); + ServoMotor::SetTargetAngle(targetAngle); + SetAngle(targetAngle); + + this->lastUpdateTimeMs = currentTimeMs; + } +} + +void ServoMotor::SetAngle(Angle16 angle) { + this->actualAngle = angle; +}; diff --git a/Things/ServoMotor.h b/Things/ServoMotor.h new file mode 100644 index 0000000..3170543 --- /dev/null +++ b/Things/ServoMotor.h @@ -0,0 +1,47 @@ +#pragma once + +#include "ControlledMotor.h" +#include "LinearAlgebra/Angle.h" + +namespace RoboidControl { + +class ServoMotor : public Thing { + public: + ServoMotor(); + + Direction16 rotationAxis = Direction16::up; + Angle16 minAngle = Angle16::Degrees(-90); + Angle16 maxAngle = Angle16::Degrees(90); + + enum ControlMode { Position, Velocity }; + ControlMode controlMode = ControlMode::Position; + + Thing* target = nullptr; + + virtual void SetTargetAngle(Angle16 angle); + virtual Angle16 GetTargetAngle(); + + virtual void SetMaximumVelocity(float maxVelocity); + + virtual void SetTargetVelocity(float velocity); + virtual float GetTargetVelocity(); + + virtual void Update(unsigned long currentTimeMs, bool recurse) override; + + Angle16 limitedTargetAngle = Angle16(); + + protected: + bool hasTargetAngle = false; + Angle16 targetAngle = Angle16(); + Angle16 actualAngle = Angle16(); + + float maxSpeed = 0.0F; + + float targetVelocity = 0.0F; + + float lastUpdateTimeMs = 0.0F; + + virtual void SetAngle(Angle16 angle); +}; + +} // namespace RoboidContol