From 5e057710b4a1900f80328a2a009ecf5e73908dda Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 5 Dec 2023 09:43:15 +0100 Subject: [PATCH] Cleanup propulsion --- DifferentialDrive.cpp | 11 ++++---- DifferentialDrive.h | 2 +- Propulsion.cpp | 66 ++++++++++++++++--------------------------- Propulsion.h | 26 ++++++----------- Quadcopter.cpp | 38 +++++++++++++------------ Quadcopter.h | 14 +++++---- 6 files changed, 68 insertions(+), 89 deletions(-) diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 9641444..2f2d1ea 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -50,8 +50,9 @@ void DifferentialDrive::SetTwistSpeed(float forward, float yaw) { SetTargetSpeeds(leftSpeed, rightSpeed); } -void DifferentialDrive::SetTwistSpeed(float forward, float yaw, float pitch) { - float leftSpeed = Float::Clamp(forward - yaw, -1, 1); - float rightSpeed = Float::Clamp(forward + yaw, -1, 1); - SetTargetSpeeds(leftSpeed, rightSpeed); -} \ No newline at end of file +// void DifferentialDrive::SetTwistSpeed(float forward, float yaw, float pitch) +// { +// float leftSpeed = Float::Clamp(forward - yaw, -1, 1); +// float rightSpeed = Float::Clamp(forward + yaw, -1, 1); +// SetTargetSpeeds(leftSpeed, rightSpeed); +// } \ No newline at end of file diff --git a/DifferentialDrive.h b/DifferentialDrive.h index 67120d9..1ad7bad 100644 --- a/DifferentialDrive.h +++ b/DifferentialDrive.h @@ -13,7 +13,7 @@ class DifferentialDrive : public Propulsion { void SetTargetSpeeds(float leftSpeed, float rightSpeed); virtual void SetTwistSpeed(float forward, float yaw) override; - virtual void SetTwistSpeed(float forward, float yaw, float pitch) override; + // virtual void SetTwistSpeed(float forward, float yaw, float pitch) override; }; } // namespace RoboidControl diff --git a/Propulsion.cpp b/Propulsion.cpp index b600c69..40fd01f 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -29,9 +29,9 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { } } -void Propulsion::AddQuadcopter(Quadcopter* quadcopter) { - this->quadcopter = quadcopter; -} +// void Propulsion::AddQuadcopter(Quadcopter* quadcopter) { +// this->quadcopter = quadcopter; +// } unsigned int Propulsion::GetMotorCount() { return this->motorCount; @@ -113,48 +113,30 @@ void Propulsion::SetMaxSpeed(float maxSpeed) { // }; // } -void Propulsion::SetTwistSpeed(float forward, float yaw) { - // This is configuration dependent, a drone will do something completely - // different... - // float leftSpeed = Float::Clamp(forward - yaw, -1, 1); - // float rightSpeed = Float::Clamp(forward + yaw, -1, 1); - // SetDiffDriveSpeed(leftSpeed, rightSpeed); +void Propulsion::SetTwistSpeed(float forward, float yaw) {} + +void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {} + +void Propulsion::SetTwistSpeed(Vector3 linear, + float yaw, + float pitch, + float roll) { + // if (quadcopter != nullptr) + // quadcopter->SetTwistSpeed(linear, yaw); + // else + // SetTwistSpeed(linear.z, yaw); } -void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) { - // float leftSpeed = Float::Clamp(forward - yaw, -1, 1); - // float rightSpeed = Float::Clamp(forward + yaw, -1, 1); - // SetDiffDriveSpeed(leftSpeed, rightSpeed); +// void Propulsion::SetLinearSpeed(Vector3 velocity, +// float yawSpeed, +// float rollSpeed) { +// if (quadcopter != nullptr) +// quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); +// } - // if (quadcopter != nullptr) { - // quadcopter->SetTwistSpeed(forward, yaw, pitch); - // } -} - -void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) { - if (quadcopter != nullptr) - quadcopter->SetTwistSpeed(linear, yaw); - else - SetTwistSpeed(linear.z, yaw); -} - -void Propulsion::SetTwistVelocity(float forwardVelocity, - float turningVelocity) { - // float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, - // 1); float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, - // -1, 1); SetDiffDriveVelocities(leftVelocity, rightVelocity); -} - -void Propulsion::SetLinearSpeed(Vector3 velocity, - float yawSpeed, - float rollSpeed) { - if (quadcopter != nullptr) - quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); -} - -Quadcopter* Propulsion::GetQuadcopter() { - return quadcopter; -} +// Quadcopter* Propulsion::GetQuadcopter() { +// return quadcopter; +// } /// @brief Odometer returns the total distance traveled since start /// @return The total distance diff --git a/Propulsion.h b/Propulsion.h index 53d5c7a..b167dd6 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -2,7 +2,7 @@ #include "ControlledMotor.h" #include "Placement.h" -#include "Quadcopter.h" +// #include "Quadcopter.h" #include "Vector2.h" // #include @@ -18,8 +18,8 @@ class Propulsion { void Update(float currentTimeMs); void AddMotors(Placement* motors, unsigned int motorCount); - void AddQuadcopter(Quadcopter* quadcopter); - Quadcopter* GetQuadcopter(); + // void AddQuadcopter(Quadcopter* quadcopter); + // Quadcopter* GetQuadcopter(); unsigned int GetMotorCount(); Motor* GetMotor(unsigned int motorIx); @@ -34,18 +34,12 @@ class Propulsion { void SetMaxSpeed(float maxSpeed); // Velocity control - // void SetDiffDriveSpeed(float leftSpeed, float rightSpeed); - // void SetDiffDriveVelocities(float leftVelocity, float rightVelocity); - - virtual void SetTwistSpeed(float forward, float yaw); - virtual void SetTwistSpeed(float forward, float yaw, float pitch); - virtual void SetTwistSpeed(Vector3 linear, float yaw); - virtual void SetTwistVelocity(float forward, float yaw); - - // Think: drones - void SetLinearSpeed(Vector3 direction, - float yawSpeed = 0.0F, - float rollSpeed = 0.0F); + virtual void SetTwistSpeed(float forward, float yaw = 0.0F); // 2D + virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F); // 2D plus + virtual void SetTwistSpeed(Vector3 linear, + float yaw = 0.0F, + float pitch = 0.0F, + float roll = 0.0F); // 3D // Position control (or actually, distance control) bool Drive(Vector3 point, float rotation, float currentTimeMs); @@ -56,8 +50,6 @@ class Propulsion { unsigned int motorCount = 0; protected: - Quadcopter* quadcopter = nullptr; - float maxSpeed = 1; bool driving = false; diff --git a/Quadcopter.cpp b/Quadcopter.cpp index 1326a91..af04794 100644 --- a/Quadcopter.cpp +++ b/Quadcopter.cpp @@ -1,36 +1,38 @@ #include "Quadcopter.h" -Quadcopter::Quadcopter() { +Quadcopter::Quadcopter() {} + +void Quadcopter::SetTwistSpeed(float forward, float yaw) { + this->velocity = Vector3::forward * forward; + this->yawSpeed = yaw; } -void Quadcopter::LinearMotion(Vector3 velocity, float yawSpeed, float rollSpeed) { - this->velocity = velocity; - this->yawSpeed = yawSpeed; - this->rollSpeed = rollSpeed; +void Quadcopter::SetTwistSpeed(Vector2 linear, float yaw) { + this->velocity = Vector3(linear.x, 0.0F, linear.y); + this->yawSpeed = yaw; } -void Quadcopter::SetTwistSpeed(Vector3 velocity, float yawSpeed) { - this->velocity = velocity; - this->yawSpeed = yawSpeed; -} - -void Quadcopter::SetTwistSpeed(float forward, float yaw, float pitch) { - this->velocity = Vector3::forward * forward; - this->yawSpeed = yaw; - this->pitchSpeed = pitch; +void Quadcopter::SetTwistSpeed(Vector3 velocity, + float yaw, + float pitch, + float roll) { + this->velocity = velocity; + this->yawSpeed = yaw; + this->rollSpeed = roll; + this->pitchSpeed = pitch; } Vector3 Quadcopter::GetTargetVelocity() { - return this->velocity; + return this->velocity; } float Quadcopter::GetPitchSpeed() { - return this->pitchSpeed; + return this->pitchSpeed; } float Quadcopter::GetYawSpeed() { - return this->yawSpeed; + return this->yawSpeed; } float Quadcopter::GetRollSpeed() { - return this->rollSpeed; + return this->rollSpeed; } diff --git a/Quadcopter.h b/Quadcopter.h index d5a2214..af2e7d3 100644 --- a/Quadcopter.h +++ b/Quadcopter.h @@ -1,20 +1,22 @@ #pragma once +#include "Propulsion.h" #include "Thing.h" #include "Vector3.h" namespace Passer { namespace RoboidControl { -class Quadcopter : public Thing { +class Quadcopter : public Propulsion { public: Quadcopter(); - void LinearMotion(Vector3 velocity, - float yawSpeed = 0.0F, - float rollSpeed = 0.0); - void SetTwistSpeed(float forward, float yaw, float pitch); - virtual void SetTwistSpeed(Vector3 velocity, float yawSpeed = 0.0F); + virtual void SetTwistSpeed(float forward, float yaw = 0.0F) override; + virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F) override; + virtual void SetTwistSpeed(Vector3 linear, + float yaw = 0.0F, + float pitch = 0.0F, + float roll = 0.0F) override; Vector3 GetTargetVelocity(); float GetYawSpeed();