Replace twist with linear/angular velocity

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; }
+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;
+}

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;

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;
+// }

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;