diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp
index 5f331cb..5396c6e 100644
--- a/ControlledMotor.cpp
+++ b/ControlledMotor.cpp
@@ -1,24 +1,45 @@
 #include "ControlledMotor.h"
-
+#include <Arduino.h>
 ControlledMotor::ControlledMotor() {
-  // this->isControlledMotor = true;
   this->type = Type::ControlledMotor;
 }
 
-ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) {
+ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
+    : ControlledMotor() {
   this->motor = motor;
   this->encoder = encoder;
 }
 
 void ControlledMotor::SetTargetSpeed(float velocity) {
   this->targetVelocity = velocity;
+  this->rotationDirection = (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
 }
 
-void ControlledMotor::Update(float timeStep) {
-  float velocity = GetActualSpeed();
+void ControlledMotor::Update(float currentTimeMs) {
+  actualVelocity = (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
   float error = targetVelocity - velocity;
 
+  float timeStep = currentTimeMs - lastUpdateTime;
   float acceleration =
-      error * timeStep * pidP;  // Just P is used at this moment
+      error * timeStep * pidP;                     // Just P is used at this moment
   motor->SetSpeed(targetVelocity + acceleration);  // or something like that
+  this->lastUpdateTime = currentTimeMs;
+}
+
+float ControlledMotor::GetActualSpeed() {
+  return actualVelocity;  //(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
+}
+
+bool ControlledMotor::Drive(float distance) {
+  if (!driving) {
+    targetDistance = distance;
+    startDistance = encoder->GetDistance();
+    driving = true;
+  }
+  // else
+  //   targetDistance = encoder->GetDistance();  // encoder->RestartCountingRevolutions();
+  float totalDistance = encoder->GetDistance() - startDistance;
+  Serial.printf("total distance = %f\n", totalDistance);
+  bool completed = totalDistance > targetDistance;
+  return completed;
 }
\ No newline at end of file
diff --git a/ControlledMotor.h b/ControlledMotor.h
index fc896a2..d60c948 100644
--- a/ControlledMotor.h
+++ b/ControlledMotor.h
@@ -17,7 +17,7 @@ class ControlledMotor : public Thing {
   float pidD = 0;
   float pidI = 0;
 
-  void Update(float timeStep);
+  void Update(float currentTimeMs);
 
   /// @brief Set the target speed for the motor controller
   /// @param speed the target in revolutions per second.
@@ -25,26 +25,22 @@ class ControlledMotor : public Thing {
 
   /// @brief Get the actual speed from the encoder
   /// @return The speed in revolutions per second
-  float GetActualSpeed() {
-    return (int)rotationDirection * encoder->GetRevolutionsPerSecond();
-  }
+  float GetActualSpeed();
 
-  bool Drive(float distance) {
-    if (!driving) {
-      targetDistance = distance;
-      encoder->StartCountingRevolutions();
-      driving = true;
-    } else
-      targetDistance -= encoder->RestartCountingRevolutions();
+  bool Drive(float distance);
 
-    return (targetDistance <= 0);
-  }
-
- protected:
-  float targetVelocity;
   Motor* motor;
   Encoder* encoder;
-  enum Direction { Forward = 1, Reverse = -1 };
+
+ protected:
+  float lastUpdateTime;
+  float targetVelocity;
+  float actualVelocity;
+  float netDistance = 0;
+  float startDistance = 0;
+
+  enum Direction { Forward = 1,
+                   Reverse = -1 };
 
   Direction rotationDirection;
 
diff --git a/Encoder.cpp b/Encoder.cpp
index 05a68bc..652b0f6 100644
--- a/Encoder.cpp
+++ b/Encoder.cpp
@@ -1,50 +1,26 @@
 #include "Encoder.h"
 
-volatile unsigned char Encoder::transitionCount = 0;
-
 Encoder::Encoder() {
-  rps = 0;
   transitionsPerRotation = 1;  // to prevent devide by zero
-  distance = 0;
 }
 
-Encoder::Encoder(unsigned char pin, unsigned char transitionsPerRotation)
+Encoder::Encoder(unsigned char transitionsPerRotation)
     : Encoder::Encoder() {
-  /// Hmm. Arduino dependent code
-  // pinMode(pin, INPUT_PULLUP);
-  // attachInterrupt(digitalPinToInterrupt(pin), InterruptHandler, CHANGE);
   this->transitionsPerRotation = transitionsPerRotation;
 }
 
-void Encoder::InterruptHandler() {
-  transitionCount++;
-}
-
-float Encoder::GetRevolutionsPerSecond() {
-  return rps;
-}
-
-void Encoder::ResetDistance() {
-  distance = 0;
-}
-
-float Encoder::GetRotationDistance() {
-  return distance;
-}
-
-void Encoder::Update(float timeStep) {
-  // Hmmm. Arduino-dependent code...
-  // noInterrupts();
-  float distanceThisUpdate = transitionCount / transitionsPerRotation;
-  transitionCount = 0;
-  // interrupts();
-
-  // float rps = distanceThisUpdate * timeStep;
-  distance += distanceThisUpdate;
-}
-
-void Encoder::StartCountingRevolutions() {}
-
-float Encoder::RestartCountingRevolutions() {
+int Encoder::GetPulseCount() {
   return 0;
-}
\ No newline at end of file
+}
+
+float Encoder::GetPulsesPerSecond(float currentTimeMs) {
+  return 0;
+}
+
+float Encoder::GetDistance() {
+  return 0;
+}
+
+float Encoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
diff --git a/Encoder.h b/Encoder.h
index fc16410..4573901 100644
--- a/Encoder.h
+++ b/Encoder.h
@@ -3,24 +3,14 @@
 class Encoder {
  public:
   Encoder();
-  Encoder(unsigned char pin, unsigned char transitionsPerRotation);
+  Encoder(unsigned char transitionsPerRotation);
 
-  float GetPulsesPerSecond();
-  float GetRevolutionsPerSecond();
+  virtual int GetPulseCount();
+  virtual float GetPulsesPerSecond(float currentTimeMs);
 
-  void ResetDistance();
-  float GetRotationDistance();
-
-  void Update(float timeStep);
-
-  virtual void StartCountingRevolutions();
-  virtual float RestartCountingRevolutions();
+  virtual float GetDistance();
+  virtual float GetRevolutionsPerSecond(float currentTimeMs);
 
  protected:
-  static void InterruptHandler();
-  static volatile unsigned char transitionCount;
   unsigned char transitionsPerRotation;
-  float rps;
-
-  float distance;  // this is direction agnostic
 };
diff --git a/Propulsion.cpp b/Propulsion.cpp
index ea7e9c5..463e03a 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -10,28 +10,21 @@ Propulsion::Propulsion() {
   this->motorCount = 0;
 }
 
-// void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
-//   this->palce = motors;
-//   this->motorCount = motorCount;
-// }
-
 void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
-  // this->placement = motors;
-  // this->motorCount = motorCount;
   this->motorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    // if (thing->isMotor)
-    if (thing->type == Thing::Type::Motor)
+    if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor)
       motorCount++;
+    if (thing->type == Thing::Type::ControlledMotor)
+      hasOdometer = true;
   }
   this->placement = new Placement[motorCount];
 
   unsigned int motorIx = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    // if (thing->isMotor)
-    if (thing->type == Thing::Type::Motor)
+    if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor)
       this->placement[motorIx++] = things[thingIx];
   }
 }
@@ -56,29 +49,48 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
   return nullptr;
 }
 
-void Propulsion::Update() {
-  // Hmmm. Arduino dependent code
-  // unsigned long curMillis = millis();
-  // float timeStep = (float)(curMillis - lastMillis) / 1000;
-  // lastMillis = curMillis;
+void Propulsion::Update(float currentTimeMs) {
+  // time_t currentTime = time(NULL);
+  float timeStep = currentTimeMs - this->lastUpdateTime;  // difftime(currentTime, this->lastUpdateTime);
 
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    // Placement placement = placement[motorIx];
-    //  placement.controlledMotor->Update(timeStep);
+    Thing* thing = placement[motorIx].thing;
+    if (thing->type == Thing::Type::ControlledMotor) {
+      ControlledMotor* motor = (ControlledMotor*)thing;
+      motor->Update(currentTimeMs);
+    }
   }
+  this->lastUpdateTime = currentTimeMs;
 }
 
 void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Motor* motor = (Motor*)placement[motorIx].thing;
-    if (motor == nullptr)
-      continue;
+    Thing* thing = placement[motorIx].thing;
+    if (thing->type == Thing::Type::Motor) {
+      Motor* motor = (Motor*)placement[motorIx].thing;
+      if (motor == nullptr)
+        continue;
 
-    float xPosition = placement[motorIx].position.x;
-    if (xPosition < 0)
-      motor->SetSpeed(leftSpeed);
-    else if (xPosition > 0)
-      motor->SetSpeed(rightSpeed);
+      float xPosition = placement[motorIx].position.x;
+      if (xPosition < 0)
+        motor->SetSpeed(leftSpeed);
+      else if (xPosition > 0)
+        motor->SetSpeed(rightSpeed);
+
+      Serial.printf("motor %d, speed = %f\n", motorIx, motor->GetSpeed());
+    } else if (thing->type == Thing::Type::ControlledMotor) {
+      ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing;
+      if (motor == nullptr)
+        continue;
+
+      float xPosition = placement[motorIx].position.x;
+      if (xPosition < 0)
+        motor->SetTargetSpeed(leftSpeed);
+      else if (xPosition > 0)
+        motor->SetTargetSpeed(rightSpeed);
+
+      Serial.printf("controlled motor %d, speed = %f\n", motorIx, motor->GetActualSpeed());
+    }
   };
 }
 
@@ -136,16 +148,39 @@ Quadcopter* Propulsion::GetQuadcopter() {
   return quadcopter;
 }
 
-bool Propulsion::Drive(Vector3 point, float rotation) {
+float Propulsion::GetOdometer() {
+  float odometer = 0;
+  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
+    Thing* thing = placement[motorIx].thing;
+    if (thing->type == Thing::Type::ControlledMotor) {
+      ControlledMotor* motor = (ControlledMotor*)thing;
+      odometer += motor->encoder->GetDistance() / this->motorCount;
+    }
+  }
+  return odometer;
+}
+
+bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
   if (!this->driving) {
     this->startTime = time(NULL);
     this->targetDistance = point.magnitude();
+    if (hasOdometer)
+      this->startOdometer = GetOdometer();
     this->driving = true;
   }
-  double duration = difftime(time(NULL), this->startTime);
-  if (duration >= this->targetDistance) {
-    this->driving = false;
-    return true;
+  if (hasOdometer) {
+    float distance = GetOdometer() - this->startOdometer;
+    Serial.printf("Odometer = %f\n", distance);
+    if (distance >= this->targetDistance) {
+      this->driving = false;
+      return true;
+    }
+  } else {
+    double duration = difftime(time(NULL), this->startTime);
+    if (duration >= this->targetDistance) {
+      this->driving = false;
+      return true;
+    }
   }
 
   if (rotation > 0)
@@ -154,5 +189,7 @@ bool Propulsion::Drive(Vector3 point, float rotation) {
     rotation = -1;
   SetTwistSpeed(point.normalized(), rotation);
 
+  Update(currentTimeMs);
+
   return false;
 }
\ No newline at end of file
diff --git a/Propulsion.h b/Propulsion.h
index c9f3d6c..dad7296 100644
--- a/Propulsion.h
+++ b/Propulsion.h
@@ -11,7 +11,7 @@ class Propulsion {
   /// @brief Setup sensing
   Propulsion();
 
-  void Update();
+  void Update(float currentTimeMs);
 
   void AddMotors(Placement* motors, unsigned int motorCount);
   void AddQuadcopter(Quadcopter* quadcopter);
@@ -35,7 +35,9 @@ class Propulsion {
                       float rollSpeed = 0.0F);
 
   // Position control (or actually, distance control)
-  bool Drive(Vector3 point, float rotation);
+  bool Drive(Vector3 point, float rotation, float currentTimeMs);
+
+  float GetOdometer();
 
   Placement* placement = nullptr;
   unsigned int motorCount = 0;
@@ -45,5 +47,8 @@ class Propulsion {
 
   bool driving = false;
   float targetDistance = 0;
+  bool hasOdometer = false;
+  float startOdometer;
   time_t startTime;
+  float lastUpdateTime;
 };
diff --git a/Roboid.cpp b/Roboid.cpp
index f01a808..7bf37ef 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -13,8 +13,8 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
   propulsion.AddMotors(configuration, thingCount);
 }
 
-bool Roboid::Drive(Waypoint* waypoint) {
-  bool finished = propulsion.Drive(waypoint->point, waypoint->rotation);
+bool Roboid::Drive(Waypoint* waypoint, float currentTimeMs) {
+  bool finished = propulsion.Drive(waypoint->point, waypoint->rotation, currentTimeMs);
   return finished;
 }
 
@@ -23,18 +23,19 @@ void Roboid::FollowTrajectory(Trajectory* trajectory) {
   this->waypointIx = 0;
 }
 
-void Roboid::Update() {
+void Roboid::Update(float currentTimeMs) {
   if (this->trajectory == nullptr)
     return;
 
   Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
-  // Serial.printf("Driving waypoints %d:  %f %f\n", this->waypointIx,
-  //               waypoint->point.z, waypoint->rotation);
-  if (Drive(waypoint)) {
+  Serial.printf("Driving waypoints %d:  %f %f\n", this->waypointIx,
+                waypoint->point.z, waypoint->rotation);
+  if (Drive(waypoint, currentTimeMs)) {
     this->waypointIx++;
     if (this->waypointIx == this->trajectory->waypointCount) {
       this->trajectory = nullptr;
       this->waypointIx = 0;
+      // stop
       propulsion.SetTwistSpeed(0, 0);
     }
   }
diff --git a/Roboid.h b/Roboid.h
index 3d0952d..3b10ec4 100644
--- a/Roboid.h
+++ b/Roboid.h
@@ -45,9 +45,9 @@ class Roboid {
   Placement* configuration;
   unsigned int thingCount;
 
-  void Update();
+  void Update(float currentTimeMs);
 
-  bool Drive(Waypoint* waypoint);
+  bool Drive(Waypoint* waypoint, float currentTimeMs);
   void FollowTrajectory(Trajectory* Trajectory);
 
  public: