Replace twist with linear/angular velocity

Propulsion.cpp

@@ -8,7 +8,9 @@ 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) {
   if (motorId >= this->motorCount)
@@ -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 {
@@ -38,6 +40,7 @@ public:
   /// 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
@@ -48,16 +51,29 @@ public:
     /// 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
+    /// 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
+    /// @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,
+    virtual void SetTwistSpeed(Vector3 linear,
-                             float pitch = 0.0F, float roll = 0.0F);
+                               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

Quadcopter.cpp

@@ -2,37 +2,37 @@
 
 Quadcopter::Quadcopter() {}
 
-void Quadcopter::SetTwistSpeed(float forward, float yaw) {
+// void Quadcopter::SetTwistSpeed(float forward, float yaw) {
-  this->velocity = Vector3::forward * forward;
+//   this->velocity = Vector3::forward * forward;
-  this->yawSpeed = yaw;
+//   this->yawSpeed = yaw;
-}
+// }
 
-void Quadcopter::SetTwistSpeed(Vector2 linear, float yaw) {
+// void Quadcopter::SetTwistSpeed(Vector2 linear, float yaw) {
-  this->velocity = Vector3(linear.x, 0.0F, linear.y);
+//   this->velocity = Vector3(linear.x, 0.0F, linear.y);
-  this->yawSpeed = yaw;
+//   this->yawSpeed = yaw;
-}
+// }
 
-void Quadcopter::SetTwistSpeed(Vector3 velocity,
+// void Quadcopter::SetTwistSpeed(Vector3 velocity,
-                               float yaw,
+//                                float yaw,
-                               float pitch,
+//                                float pitch,
-                               float roll) {
+//                                float roll) {
-  this->velocity = velocity;
+//   this->velocity = velocity;
-  this->yawSpeed = yaw;
+//   this->yawSpeed = yaw;
-  this->rollSpeed = roll;
+//   this->rollSpeed = roll;
-  this->pitchSpeed = pitch;
+//   this->pitchSpeed = pitch;
-}
+// }
 
-Vector3 Quadcopter::GetTargetVelocity() {
+// Vector3 Quadcopter::GetTargetVelocity() {
-  return this->velocity;
+//   return this->velocity;
-}
+// }
 
-float Quadcopter::GetPitchSpeed() {
+// float Quadcopter::GetPitchSpeed() {
-  return this->pitchSpeed;
+//   return this->pitchSpeed;
-}
+// }
 
-float Quadcopter::GetYawSpeed() {
+// float Quadcopter::GetYawSpeed() {
-  return this->yawSpeed;
+//   return this->yawSpeed;
-}
+// }
-float Quadcopter::GetRollSpeed() {
+// float Quadcopter::GetRollSpeed() {
-  return this->rollSpeed;
+//   return this->rollSpeed;
-}
+// }

Quadcopter.h

@@ -14,15 +14,16 @@ public:
   /// @brief Default constuctor
   Quadcopter();
 
-  virtual void SetTwistSpeed(float forward, float yaw = 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(Vector2 linear, float yaw = 0.0F) override;
-  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
+  //   virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
-                             float pitch = 0.0F, float roll = 0.0F) override;
+  //                              float pitch = 0.0F, float roll = 0.0F)
+  //                              override;
 
-  Vector3 GetTargetVelocity();
+  //   Vector3 GetTargetVelocity();
-  float GetYawSpeed();
+  //   float GetYawSpeed();
-  float GetPitchSpeed();
+  //   float GetPitchSpeed();
-  float GetRollSpeed();
+  //   float GetRollSpeed();
 
  protected:
   Vector3 velocity = Vector3::zero;