Cleanup propulsion

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) {
+// 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);
+//   float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
-  SetTargetSpeeds(leftSpeed, rightSpeed);
+//   float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
-}
+//   SetTargetSpeeds(leftSpeed, rightSpeed);
+// }

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

Propulsion.cpp

@@ -29,9 +29,9 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
   }
 }
 
-void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
+// void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
-  this->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) {
+void Propulsion::SetTwistSpeed(float forward, float yaw) {}
-  // This is configuration dependent, a drone will do something completely
+
-  // different...
+void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
-  // float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
+
-  // float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
+void Propulsion::SetTwistSpeed(Vector3 linear,
-  // SetDiffDriveSpeed(leftSpeed, rightSpeed);
+                               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) {
+// void Propulsion::SetLinearSpeed(Vector3 velocity,
-  // float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
+//                                 float yawSpeed,
-  // float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
+//                                 float rollSpeed) {
-  // SetDiffDriveSpeed(leftSpeed, rightSpeed);
+//   if (quadcopter != nullptr)
-
+//     quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
-  // if (quadcopter != nullptr) {
-  //   quadcopter->SetTwistSpeed(forward, yaw, pitch);
 // }
-}
 
-void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
+// Quadcopter* Propulsion::GetQuadcopter() {
-  if (quadcopter != nullptr)
+//   return quadcopter;
-    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;
-}
 
 /// @brief Odometer returns the total distance traveled since start
 /// @return The total distance

Propulsion.h

@@ -2,7 +2,7 @@
 
 #include "ControlledMotor.h"
 #include "Placement.h"
-#include "Quadcopter.h"
+// #include "Quadcopter.h"
 #include "Vector2.h"
 
 // #include <time.h>
@@ -18,8 +18,8 @@ class Propulsion {
   void Update(float currentTimeMs);
 
   void AddMotors(Placement* motors, unsigned int motorCount);
-  void AddQuadcopter(Quadcopter* quadcopter);
+  // void AddQuadcopter(Quadcopter* quadcopter);
-  Quadcopter* GetQuadcopter();
+  // 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);
+  virtual void SetTwistSpeed(float forward, float yaw = 0.0F);   // 2D
-  // void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
+  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);  // 2D plus
-
+  virtual void SetTwistSpeed(Vector3 linear,
-  virtual void SetTwistSpeed(float forward, float yaw);
+                             float yaw = 0.0F,
-  virtual void SetTwistSpeed(float forward, float yaw, float pitch);
+                             float pitch = 0.0F,
-  virtual void SetTwistSpeed(Vector3 linear, float yaw);
+                             float roll = 0.0F);  // 3D
-  virtual void SetTwistVelocity(float forward, float yaw);
-
-  // Think: drones
-  void SetLinearSpeed(Vector3 direction,
-                      float yawSpeed = 0.0F,
-                      float rollSpeed = 0.0F);
 
   // 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;

Quadcopter.cpp

@@ -1,22 +1,24 @@
 #include "Quadcopter.h"
 
-Quadcopter::Quadcopter() {
+Quadcopter::Quadcopter() {}
-}
 
-void Quadcopter::LinearMotion(Vector3 velocity, float yawSpeed, float rollSpeed) {
+void Quadcopter::SetTwistSpeed(float forward, float yaw) {
-	this->velocity = velocity;
-	this->yawSpeed = yawSpeed;
-	this->rollSpeed = rollSpeed;
-}
-
-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;
+}
+
+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;
 }
 

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,
+  virtual void SetTwistSpeed(float forward, float yaw = 0.0F) override;
-                    float yawSpeed = 0.0F,
+  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F) override;
-                    float rollSpeed = 0.0);
+  virtual void SetTwistSpeed(Vector3 linear,
-  void SetTwistSpeed(float forward, float yaw, float pitch);
+                             float yaw = 0.0F,
-  virtual void SetTwistSpeed(Vector3 velocity, float yawSpeed = 0.0F);
+                             float pitch = 0.0F,
+                             float roll = 0.0F) override;
 
   Vector3 GetTargetVelocity();
   float GetYawSpeed();