diff --git a/Motor.cpp b/Motor.cpp
index a8f8a60..76f8646 100644
--- a/Motor.cpp
+++ b/Motor.cpp
@@ -4,30 +4,9 @@
 #include <Arduino.h>
 
 Motor::Motor() {
-  //   this->isMotor = true;
   type = Type::Motor;
 }
 
-// Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) {
-//   this->pinIn1 = pinIn1;
-//   this->pinIn2 = pinIn2;
-
-//   pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
-//   pinMode(pinIn2, OUTPUT);  // configure the in2 pin to output mode
-// }
-
-// void Motor::SetDirection(Direction direction) {
-//   digitalWrite(pinIn1, direction);
-//   digitalWrite(pinIn2, !direction);  // This is the opposite of pinIn1
-// }
-
-// void Motor::SetSpeed(float speed) {  // 0..1
-//   currentSpeed = speed;
-//   uint8_t motorSignal = (uint8_t)(speed * 255);
-//   analogWrite(pinIn1, speed);
-//   analogWrite(pinIn2, 255 - speed);
-// }
-
 float Motor::GetSpeed() {
   return this->currentSpeed;
 }
@@ -48,6 +27,6 @@ bool Motor::Drive(float distance) {
     return true;
   }
 
-  SetSpeed(distance < 0 ? -1 : 1);
+  SetSpeed(distance < 0 ? -1 : 1);  // max speed
   return false;
 }
\ No newline at end of file
diff --git a/Propulsion.cpp b/Propulsion.cpp
index 612817e..ea7e9c5 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -3,6 +3,8 @@
 
 #include "FloatSingle.h"
 
+#include <Arduino.h>
+
 Propulsion::Propulsion() {
   this->placement = nullptr;
   this->motorCount = 0;
@@ -112,6 +114,8 @@ void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
 void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
   if (quadcopter != nullptr)
     quadcopter->SetTwistSpeed(linear, yaw);
+  else
+    SetTwistSpeed(linear.z, yaw);
 }
 
 void Propulsion::SetTwistVelocity(float forwardVelocity,
@@ -132,20 +136,23 @@ Quadcopter* Propulsion::GetQuadcopter() {
   return quadcopter;
 }
 
-// bool Propulsion::Drive(Roboid* roboid, float forwardDistance) {
-//   bool finished = true;
-//   for (unsigned int motorIx = 0; motorIx < roboid->propulsion->motorCount;
-//        motorIx++) {
-//     Motor* motor = (Motor*)roboid->placement[motorIx].thing;
-//     if (motor == nullptr)
-//       continue;
-//     if (motor->isControlledMotor == false)
-//       continue;
+bool Propulsion::Drive(Vector3 point, float rotation) {
+  if (!this->driving) {
+    this->startTime = time(NULL);
+    this->targetDistance = point.magnitude();
+    this->driving = true;
+  }
+  double duration = difftime(time(NULL), this->startTime);
+  if (duration >= this->targetDistance) {
+    this->driving = false;
+    return true;
+  }
 
-//     ControlledMotor* controlledMotor = (ControlledMotor*)motor;
-//     bool motorFinished = controlledMotor->Drive(forwardDistance);
-//     if (motorFinished == false)
-//       finished = false;
-//   }
-//   return finished;
-// }
+  if (rotation > 0)
+    rotation = 1;
+  if (rotation < 0)
+    rotation = -1;
+  SetTwistSpeed(point.normalized(), rotation);
+
+  return false;
+}
\ No newline at end of file
diff --git a/Propulsion.h b/Propulsion.h
index 8da9003..c9f3d6c 100644
--- a/Propulsion.h
+++ b/Propulsion.h
@@ -4,7 +4,7 @@
 #include "Quadcopter.h"
 #include "Vector2.h"
 
-// #include <list>
+#include <time.h>
 
 class Propulsion {
  public:
@@ -35,10 +35,7 @@ class Propulsion {
                       float rollSpeed = 0.0F);
 
   // Position control (or actually, distance control)
-  // void Drive(float forwardDistance);
-  // void Turn(float turnDistance);
-
-  // void Update();
+  bool Drive(Vector3 point, float rotation);
 
   Placement* placement = nullptr;
   unsigned int motorCount = 0;
@@ -48,4 +45,5 @@ class Propulsion {
 
   bool driving = false;
   float targetDistance = 0;
+  time_t startTime;
 };
diff --git a/Roboid.cpp b/Roboid.cpp
index 00223c2..f01a808 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -13,56 +13,8 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
   propulsion.AddMotors(configuration, thingCount);
 }
 
-// bool Roboid::Drive(float forwardDistance) {
-//   bool finished = true;
-//   for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++)
-//   {
-//     bool motorFinished = false;
-//     Thing* thing = propulsion.placement[motorIx].thing;
-
-//     switch (thing->type) {
-//       case Thing::Type::ControlledMotor: {
-//         ControlledMotor* controlledMotor = (ControlledMotor*)thing;
-//         motorFinished = controlledMotor->Drive(forwardDistance);
-//         break;
-//       }
-//       case Thing::Type::Motor: {
-//         Motor* motor = (Motor*)thing;
-//         motorFinished = motor->Drive(forwardDistance);
-//         break;
-//       }
-//       default:
-//         break;
-//     }
-//     if (motorFinished == false)
-//       finished = false;
-//   }
-//   return finished;
-// }
-
 bool Roboid::Drive(Waypoint* waypoint) {
-  bool finished = true;
-  for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
-    bool motorFinished = false;
-    Thing* thing = propulsion.placement[motorIx].thing;
-
-    switch (thing->type) {
-      case Thing::Type::ControlledMotor: {
-        ControlledMotor* controlledMotor = (ControlledMotor*)thing;
-        motorFinished = controlledMotor->Drive(waypoint->point.z);
-        break;
-      }
-      case Thing::Type::Motor: {
-        Motor* motor = (Motor*)thing;
-        motorFinished = motor->Drive(waypoint->point.z);
-        break;
-      }
-      default:
-        break;
-    }
-    if (motorFinished == false)
-      finished = false;
-  }
+  bool finished = propulsion.Drive(waypoint->point, waypoint->rotation);
   return finished;
 }
 
diff --git a/Roboid.h b/Roboid.h
index c8f9c37..3d0952d 100644
--- a/Roboid.h
+++ b/Roboid.h
@@ -47,7 +47,6 @@ class Roboid {
 
   void Update();
 
-  // bool Drive(float forwardDistance);
   bool Drive(Waypoint* waypoint);
   void FollowTrajectory(Trajectory* Trajectory);