From edf264c302611c9c044646efb483ac31451bf778 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 9 Jan 2024 12:19:26 +0100 Subject: [PATCH] Encoder can evaluate velocity --- ControlledMotor.cpp | 33 ++++++++++++++++++-------- ControlledMotor.h | 16 ++++++------- DifferentialDrive.cpp | 54 +++++++++++++++++++++++++++++++------------ DifferentialDrive.h | 3 +++ Motor.cpp | 12 ++++------ Motor.h | 6 +++-- Propulsion.cpp | 12 ++++++++-- Roboid.cpp | 2 ++ 8 files changed, 95 insertions(+), 43 deletions(-) diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp index 79e7aae..87c04c6 100644 --- a/ControlledMotor.cpp +++ b/ControlledMotor.cpp @@ -6,26 +6,41 @@ ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder) : ControlledMotor() { this->motor = motor; this->encoder = encoder; + // this->rotationDirection = Direction::Forward; } -void ControlledMotor::SetTargetSpeed(float velocity) { - this->targetVelocity = velocity; - this->rotationDirection = - (targetVelocity < 0) ? Direction::Reverse : Direction::Forward; +#include + +void ControlledMotor::SetTargetSpeed(float speed) { + this->targetSpeed = speed; + // this->rotationDirection = + // (targetSpeed < 0) ? Direction::Reverse : Direction::Forward; + // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise + // : Motor::Direction::Clockwise; } void ControlledMotor::Update(float currentTimeMs) { - actualVelocity = - (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs); - float error = targetVelocity - velocity; + actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs); + if (this->targetSpeed < 0) + actualSpeed = -actualSpeed; + + // Serial.print("direction "); + // Serial.print(this->direction); + // Serial.print(" actualSpeed "); + // Serial.println(actualSpeed); + // Serial.print(" target "); + // Serial.println(this->targetSpeed); + + float error = targetSpeed - velocity; float timeStep = currentTimeMs - lastUpdateTime; float acceleration = error * timeStep * pidP; // Just P is used at this moment - motor->SetSpeed(targetVelocity + acceleration); // or something like that + motor->SetTargetSpeed( + this->targetSpeed); // + acceleration); // or something like that this->lastUpdateTime = currentTimeMs; } -float ControlledMotor::GetActualSpeed() { return actualVelocity; } +float ControlledMotor::GetActualSpeed() { return actualSpeed; } bool ControlledMotor::Drive(float distance) { if (!driving) { diff --git a/ControlledMotor.h b/ControlledMotor.h index a522b7b..48bbfb4 100644 --- a/ControlledMotor.h +++ b/ControlledMotor.h @@ -9,7 +9,7 @@ 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 Thing { +class ControlledMotor : public Motor { public: ControlledMotor(); ControlledMotor(Motor *motor, Encoder *encoder); @@ -23,15 +23,15 @@ public: float pidD = 0; float pidI = 0; - void Update(float currentTimeMs); + void Update(float currentTimeMs) override; /// @brief Set the target speed for the motor controller /// @param speed the target in revolutions per second. - virtual void SetTargetSpeed(float speed); + virtual void SetTargetSpeed(float speed) override; /// @brief Get the actual speed from the encoder /// @return The speed in revolutions per second - virtual float GetActualSpeed(); + virtual float GetActualSpeed() override; bool Drive(float distance); @@ -40,14 +40,14 @@ public: protected: float lastUpdateTime; - float targetVelocity; - float actualVelocity; + float targetSpeed; + float actualSpeed; float netDistance = 0; float startDistance = 0; - enum Direction { Forward = 1, Reverse = -1 }; + // enum Direction { Forward = 1, Reverse = -1 }; - Direction rotationDirection; + // Direction rotationDirection; bool driving = false; float targetDistance = 0; diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 7d64d9b..321af55 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -12,7 +12,8 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor, this->motors[0] = leftMotor; this->motors[1] = rightMotor; - float distance = separation / 2; + this->wheelSeparation = separation; + float distance = wheelSeparation / 2; leftMotor->direction = Motor::Direction::CounterClockwise; leftMotor->position.angle = -90; leftMotor->position.distance = distance; @@ -21,19 +22,19 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor, rightMotor->position.distance = distance; } +#include + void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { Motor *motor = motors[motorIx]; if (motor == nullptr) continue; - if (motor->type == Thing::UncontrolledMotorType) { - float xPosition = motors[motorIx]->position.angle; - if (xPosition < 0) - motor->SetSpeed(leftSpeed); - else if (xPosition > 0) - motor->SetSpeed(rightSpeed); - } + float xPosition = motors[motorIx]->position.angle; + if (xPosition < 0) + motor->SetTargetSpeed(leftSpeed); + else if (xPosition > 0) + motor->SetTargetSpeed(rightSpeed); }; } @@ -52,23 +53,46 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch, SetTwistSpeed(linear.z, yaw); } +#include + Polar DifferentialDrive::GetVelocity() { Motor *leftMotor = motors[0]; Motor *rightMotor = motors[1]; - float leftSpeed = leftMotor->GetSpeed(); - float rightSpeed = rightMotor->GetSpeed(); + float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second + float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second + leftSpeed = leftSpeed * wheelDiameter * M_PI; // in meters per second + rightSpeed = rightSpeed * wheelDiameter * M_PI; // in meters per second float speed = (leftSpeed + rightSpeed) / 2; + float direction = speed >= 0 ? 0.0F : 180.0F; - float distance = fabsf(speed); - Polar velocity = Polar(direction, distance); + float magnitude = fabsf(speed); + Polar velocity = Polar(direction, magnitude); return velocity; } float DifferentialDrive::GetAngularVelocity() { Motor *leftMotor = motors[0]; Motor *rightMotor = motors[1]; - float leftSpeed = leftMotor->GetSpeed(); - float rightSpeed = rightMotor->GetSpeed(); - float angularVelocity = leftSpeed - rightSpeed; + float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second + float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second + leftSpeed = leftSpeed * wheelDiameter * M_PI; // in meters per second + rightSpeed = rightSpeed * wheelDiameter * M_PI; // in meters per second + + float angularSpeed = (rightSpeed - leftSpeed) / 2; + float angularDistance = wheelSeparation / 2 * M_PI; + float rotationsPerSecond = angularSpeed / angularDistance; + float degreesPerSecond = 360 * rotationsPerSecond; + float angularVelocity = degreesPerSecond; + + // Serial.print(leftSpeed); + // Serial.print(" - "); + // Serial.println(rightSpeed); + + // Serial.print(angularSpeed); + // Serial.print(" "); + // Serial.print(angularDistance); + // Serial.print(" "); + // Serial.println(rotationsPerSecond); + return angularVelocity; } \ No newline at end of file diff --git a/DifferentialDrive.h b/DifferentialDrive.h index 8bcbadc..919a044 100644 --- a/DifferentialDrive.h +++ b/DifferentialDrive.h @@ -73,6 +73,9 @@ public: /// @remark This will be more expanded/detailed in a future version of Roboid /// Control virtual float GetAngularVelocity() override; + + float wheelDiameter = 1.0F; // in meters + float wheelSeparation = 1.0F; // in meters; }; } // namespace RoboidControl diff --git a/Motor.cpp b/Motor.cpp index 05ad7f0..e45dc6e 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -2,14 +2,12 @@ #include -Motor::Motor() { - type = (int)Thing::UncontrolledMotorType; -} +Motor::Motor() { type = (int)Thing::UncontrolledMotorType; } -float Motor::GetSpeed() { - return this->currentTargetSpeed; -} +float Motor::GetActualSpeed() { return this->currentTargetSpeed; } -void Motor::SetSpeed(float targetSpeed) { +void Motor::SetTargetSpeed(float targetSpeed) { this->currentTargetSpeed = targetSpeed; } + +void Motor::Update(float currentTimeMs) {} \ No newline at end of file diff --git a/Motor.h b/Motor.h index afe7d9f..8890bfa 100644 --- a/Motor.h +++ b/Motor.h @@ -22,11 +22,13 @@ public: /// @brief Set the target motor speed /// @param speed The speed between -1 (full backward), 0 (stop) and 1 (full /// forward) - virtual void SetSpeed(float speed); + 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 GetSpeed(); + virtual float GetActualSpeed(); + + virtual void Update(float currentTimeMs); protected: float currentTargetSpeed = 0; diff --git a/Propulsion.cpp b/Propulsion.cpp index deab295..337533a 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -4,7 +4,7 @@ #include "VectorAlgebra/FloatSingle.h" Propulsion::Propulsion() { - this->motors == nullptr; + this->motors = nullptr; this->motorCount = 0; } @@ -18,7 +18,15 @@ Motor *Propulsion::GetMotor(unsigned int motorId) { return motor; } -void Propulsion::Update(float currentTimeMs) {} +void Propulsion::Update(float currentTimeMs) { + for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) { + Motor *motor = this->motors[motorIx]; + if (motor == nullptr) + continue; + + motor->Update(currentTimeMs); + } +} void Propulsion::SetTwistSpeed(float forward, float yaw) {} diff --git a/Roboid.cpp b/Roboid.cpp index 3665af6..f236fef 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -16,6 +16,8 @@ Roboid::Roboid(ServoMotor **actuation) : actuation(actuation) {} void Roboid::Update(float currentTimeMs) { if (perception != nullptr) perception->Update(currentTimeMs); + if (propulsion != nullptr) + propulsion->Update(currentTimeMs); if (networkSync != nullptr) networkSync->NetworkUpdate(this); }