From 2e042981fcd283a26cb76d8a1ccbfe7c7b3a7b63 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens <21057728+passervr@users.noreply.github.com> Date: Mon, 22 Jan 2024 11:55:07 +0100 Subject: [PATCH] Replace twist with linear/angular velocity --- Propulsion.cpp | 29 ++++++++++++++++----- Propulsion.h | 70 +++++++++++++++++++++++++++++++------------------- Quadcopter.cpp | 58 ++++++++++++++++++++--------------------- Quadcopter.h | 25 +++++++++--------- 4 files changed, 108 insertions(+), 74 deletions(-) diff --git a/Propulsion.cpp b/Propulsion.cpp index 337533a..5b99852 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -8,19 +8,21 @@ Propulsion::Propulsion() { this->motorCount = 0; } -unsigned int Propulsion::GetMotorCount() { return this->motorCount; } +unsigned int Propulsion::GetMotorCount() { + return this->motorCount; +} -Motor *Propulsion::GetMotor(unsigned int motorId) { +Motor* Propulsion::GetMotor(unsigned int motorId) { if (motorId >= this->motorCount) return nullptr; - Motor *motor = this->motors[motorId]; + Motor* motor = this->motors[motorId]; return motor; } void Propulsion::Update(float currentTimeMs) { for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) { - Motor *motor = this->motors[motorIx]; + Motor* motor = this->motors[motorIx]; if (motor == nullptr) continue; @@ -28,13 +30,28 @@ void Propulsion::Update(float currentTimeMs) { } } +/* 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) {} +*/ -Polar Propulsion::GetVelocity() { return Polar(0, 0); } +void Propulsion::SetVelocity(float velocity) {} +void Propulsion::SetVelocity(Vector2 velocity) {} +void Propulsion::SetVelocity(Vector3 verlocity) {} +void Propulsion::SetVelocity(Polar velocity) {} +void Propulsion::SetVelocity(Spherical velocity) {} -float Propulsion::GetAngularVelocity() { return 0; } \ No newline at end of file +void Propulsion::SetAngularVelocity(float yaw) {} +void Propulsion::SetAngularVelocity(float yaw, float pitch, float roll) {} + +Polar Propulsion::GetVelocity() { + return Polar(0, 0); +} + +float Propulsion::GetAngularVelocity() { + return 0; +} \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index 62c5c85..8781a90 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -3,7 +3,9 @@ #include "Motor.h" #include "VectorAlgebra/Polar.h" #include "VectorAlgebra/Quaternion.h" +#include "VectorAlgebra/Spherical.h" #include "VectorAlgebra/Vector2.h" +#include "VectorAlgebra/Vector3.h" namespace Passer { namespace RoboidControl { @@ -16,7 +18,7 @@ class Roboid; /// for a robot. This base class does not implement the functions to move the /// Roboid around. class Propulsion { -public: + public: /// @brief Default Constructor for Propulsion Propulsion(); @@ -31,33 +33,47 @@ public: /// @param motorIx The index of the motor /// @return Returns the motor or a nullptr when no motor with the given index /// could be found - Motor *GetMotor(unsigned int motorIx); + Motor* GetMotor(unsigned int motorIx); /// @brief Get the Placement of a specific Motor /// @param motorIx The index of the Motor /// @return Returns the Placement or a nullptr when no Placement with the give /// index could be found // Placement *GetMotorPlacement(unsigned int motorIx); - /// @brief Sets the forward and rotation speed of a (grounded) Roboid - /// @param forward The target forward speed - /// @param yaw The target rotation speed around the vertical axis - /// This function is typically used for Roboid which are driving on the - /// ground. - virtual void SetTwistSpeed(float forward, float yaw = 0.0F); - /// @brief Sets the forward, sideward and rotation speed of a (grounded) - /// Roboid - /// @param linear The target linear (forward, sideward) speed - /// @param yaw The target rotation speed around the vertical axis - /// This function is typically used for Roboid which are driving on the ground - /// which have to ability to move sideward - virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F); - /// @brief Set the target 3D linear and 3D rotation speed of a (flying) Roboid - /// @param linear The target linear speed - /// @param yaw The target rotation speed around the vertical axis - /// @param pitch The target rotation speed around the sideward axis - /// @param roll The target rotation speed around hte forward axis - virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F, - float pitch = 0.0F, float roll = 0.0F); + /* + /// @brief Sets the forward and rotation speed of a (grounded) Roboid + /// @param forward The target forward speed + /// @param yaw The target rotation speed around the vertical axis + /// This function is typically used for Roboid which are driving on the + /// ground. + virtual void SetTwistSpeed(float forward, float yaw = 0.0F); + /// @brief Sets the forward, sideward and rotation speed of a (grounded) + /// Roboid + /// @param linear The target linear (forward, sideward) speed + /// @param yaw The target rotation speed around the vertical axis + /// This function is typically used for Roboid which are driving on the + ground + /// which have to ability to move sideward + virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F); + /// @brief Set the target 3D linear and 3D rotation speed of a (flying) + Roboid + /// @param linear The target linear speed + /// @param yaw The target rotation speed around the vertical axis + /// @param pitch The target rotation speed around the sideward axis + /// @param roll The target rotation speed around hte forward axis + virtual void SetTwistSpeed(Vector3 linear, + float yaw = 0.0F, + float pitch = 0.0F, + float roll = 0.0F); + */ + virtual void SetVelocity(float velocity); + virtual void SetVelocity(Polar velocity); + virtual void SetVelocity(Spherical velocity); + virtual void SetVelocity(Vector2 velocity); + virtual void SetVelocity(Vector3 velocity); + + virtual void SetAngularVelocity(float yaw); + virtual void SetAngularVelocity(float yaw, float pitch, float roll); /// @brief Retrieve the current velocity of the roboid /// @return The velocity in polar coordinates @@ -69,16 +85,16 @@ public: virtual float GetAngularVelocity(); /// @brief The roboid of this propulsion system - Roboid *roboid = nullptr; + Roboid* roboid = nullptr; -protected: + protected: /// @brief The number of motors used for Propulsion unsigned int motorCount = 0; /// @brief The Placement of the motors used for Propulsion // Placement *placement = nullptr; - Motor **motors = nullptr; + Motor** motors = nullptr; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Quadcopter.cpp b/Quadcopter.cpp index af04794..c05170b 100644 --- a/Quadcopter.cpp +++ b/Quadcopter.cpp @@ -2,37 +2,37 @@ Quadcopter::Quadcopter() {} -void Quadcopter::SetTwistSpeed(float forward, float yaw) { - this->velocity = Vector3::forward * forward; - this->yawSpeed = yaw; -} +// void Quadcopter::SetTwistSpeed(float forward, float yaw) { +// this->velocity = Vector3::forward * forward; +// this->yawSpeed = yaw; +// } -void Quadcopter::SetTwistSpeed(Vector2 linear, float yaw) { - this->velocity = Vector3(linear.x, 0.0F, linear.y); - this->yawSpeed = yaw; -} +// 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 yaw, - float pitch, - float roll) { - this->velocity = velocity; - this->yawSpeed = yaw; - this->rollSpeed = roll; - 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; -} +// Vector3 Quadcopter::GetTargetVelocity() { +// return this->velocity; +// } -float Quadcopter::GetPitchSpeed() { - return this->pitchSpeed; -} +// float Quadcopter::GetPitchSpeed() { +// return this->pitchSpeed; +// } -float Quadcopter::GetYawSpeed() { - return this->yawSpeed; -} -float Quadcopter::GetRollSpeed() { - return this->rollSpeed; -} +// float Quadcopter::GetYawSpeed() { +// return this->yawSpeed; +// } +// float Quadcopter::GetRollSpeed() { +// return this->rollSpeed; +// } diff --git a/Quadcopter.h b/Quadcopter.h index 5419c90..e213910 100644 --- a/Quadcopter.h +++ b/Quadcopter.h @@ -10,27 +10,28 @@ namespace RoboidControl { /// @brief Support for Quadcopter as a propulsion method /// @note This is work in progress class Quadcopter : public Propulsion { -public: + public: /// @brief Default constuctor Quadcopter(); - 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; + // 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(); - float GetPitchSpeed(); - float GetRollSpeed(); + // Vector3 GetTargetVelocity(); + // float GetYawSpeed(); + // float GetPitchSpeed(); + // float GetRollSpeed(); -protected: + protected: Vector3 velocity = Vector3::zero; float pitchSpeed = 0.0F; float yawSpeed = 0.0F; float rollSpeed = 0.0F; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file