Update quadcopter support

Propulsion.cpp

@@ -33,8 +33,8 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
 
 }
 
-void Propulsion::AddQuadcopter() {
-    this->quadcopter = new Quadcopter();
+void Propulsion::AddQuadcopter(Quadcopter* quadcoper) {
+    this->quadcopter = quadcopter;
 }
 
 unsigned int Propulsion::GetMotorCount() {
@@ -105,6 +105,11 @@ void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
   }
 }
 
+void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
+    if (quadcopter != nullptr)
+        quadcopter->SetTwistSpeed(linear, yaw);
+}
+
 void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
   float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1);
   float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1);

Propulsion.h

@@ -22,7 +22,7 @@ class Propulsion {
 
   //void AddMotors(MotorPlacement* motors, unsigned int motorCount);
   void AddMotors(Placement* motors, unsigned int motorCount);
-  void AddQuadcopter();
+  void AddQuadcopter(Quadcopter* quadcopter);
 
   unsigned int GetMotorCount();
   Motor* GetMotor(unsigned int motorIx);
@@ -32,6 +32,7 @@ class Propulsion {
 
   void SetTwistSpeed(float forward, float yaw);
   void SetTwistSpeed(float forward, float yaw, float pitch);
+  void SetTwistSpeed(Vector3 linear, float yaw);
   void SetTwistVelocity(float forward, float yaw);
 
   // Think: drones

Quadcopter.cpp

@@ -9,6 +9,11 @@ void Quadcopter::LinearMotion(Vector3 velocity, float yawSpeed, float rollSpeed)
 	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;

Quadcopter.h

@@ -9,6 +9,7 @@ public:
 
 	void LinearMotion(Vector3 velocity, float yawSpeed = 0.0F, float rollSpeed = 0.0);
 	void SetTwistSpeed(float forward, float yaw, float pitch);
+    void SetTwistSpeed(Vector3 velocity, float yawSpeed = 0.0F);
 
 	Vector3 GetTargetVelocity();
 	float GetYawSpeed();