diff --git a/Encoder.cpp b/Encoder.cpp
index 652b0f6..fb39eaa 100644
--- a/Encoder.cpp
+++ b/Encoder.cpp
@@ -1,26 +1,28 @@
 #include "Encoder.h"
 
-Encoder::Encoder() {
-  transitionsPerRotation = 1;  // to prevent devide by zero
-}
-
-Encoder::Encoder(unsigned char transitionsPerRotation)
-    : Encoder::Encoder() {
-  this->transitionsPerRotation = transitionsPerRotation;
+Encoder::Encoder(unsigned char transitionsPerRotation,
+                 unsigned char distancePerRotation) {
+  //: Encoder::Encoder() {
+  this->transitionsPerRevolution = transitionsPerRotation;
+  this->distancePerRevolution = distancePerRotation;
 }
 
 int Encoder::GetPulseCount() {
   return 0;
 }
 
-float Encoder::GetPulsesPerSecond(float currentTimeMs) {
-  return 0;
-}
-
 float Encoder::GetDistance() {
   return 0;
 }
 
+float Encoder::GetPulsesPerSecond(float currentTimeMs) {
+  return 0;
+}
+
 float Encoder::GetRevolutionsPerSecond(float currentTimeMs) {
   return 0;
 }
+
+float Encoder::GetSpeed(float currentTimeMs) {
+  return 0;
+}
\ No newline at end of file
diff --git a/Encoder.h b/Encoder.h
index ce218ad..8ecffef 100644
--- a/Encoder.h
+++ b/Encoder.h
@@ -3,19 +3,47 @@
 namespace Passer {
 namespace RoboidControl {
 
+/// @brief An Encoder measures the rotations of an axle using a rotary sensor
+/// Some encoders are able to detect direction, while others can not.
 class Encoder {
  public:
-  Encoder();
-  Encoder(unsigned char transitionsPerRotation);
+  /// @brief Creates a sensor which measures distance from pulses
+  /// @param transitionsPerRevolution The number of pulse edges which make a
+  /// full rotation
+  /// @param distancePerRevolution The distance a wheel travels per full
+  /// rotation
+  Encoder(unsigned char transitionsPerRevolution = 1,
+          unsigned char distancePerRevolution = 1);
 
+  /// @brief Get the total number of pulses since the previous call
+  /// @return The number of pulses, is zero or greater
   virtual int GetPulseCount();
+  /// @brief Get the pulse speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The average pulses per second in the last period.
   virtual float GetPulsesPerSecond(float currentTimeMs);
 
+  /// @brief Get the distance traveled since the previous call
+  /// @return The distance in meters.
   virtual float GetDistance();
+
+  /// @brief Get the rotation speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in rotations per second
   virtual float GetRevolutionsPerSecond(float currentTimeMs);
 
- protected:
-  unsigned char transitionsPerRotation;
+  /// @brief Get the speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in meters per second.
+  /// @note this value is dependent on the accurate setting of the
+  /// transitionsPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char transitionsPerRevolution = 1;
+  /// @brief The number of revolutions which makes the wheel move forward 1
+  /// meter
+  unsigned char distancePerRevolution = 1;
 };
 
 }  // namespace RoboidControl
diff --git a/Propulsion.cpp b/Propulsion.cpp
index 463e03a..5291985 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -14,7 +14,8 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
   this->motorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor)
+    if (thing->type == Thing::Type::Motor ||
+        thing->type == Thing::Type::ControlledMotor)
       motorCount++;
     if (thing->type == Thing::Type::ControlledMotor)
       hasOdometer = true;
@@ -24,7 +25,8 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
   unsigned int motorIx = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor)
+    if (thing->type == Thing::Type::Motor ||
+        thing->type == Thing::Type::ControlledMotor)
       this->placement[motorIx++] = things[thingIx];
   }
 }
@@ -51,7 +53,9 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
 
 void Propulsion::Update(float currentTimeMs) {
   // time_t currentTime = time(NULL);
-  float timeStep = currentTimeMs - this->lastUpdateTime;  // difftime(currentTime, this->lastUpdateTime);
+  float timeStep =
+      currentTimeMs -
+      this->lastUpdateTime;  // difftime(currentTime, this->lastUpdateTime);
 
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     Thing* thing = placement[motorIx].thing;
@@ -63,6 +67,10 @@ void Propulsion::Update(float currentTimeMs) {
   this->lastUpdateTime = currentTimeMs;
 }
 
+void Propulsion::SetMaxSpeed(float maxSpeed) {
+  this->maxSpeed = abs(maxSpeed);
+}
+
 void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     Thing* thing = placement[motorIx].thing;
@@ -89,7 +97,8 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
       else if (xPosition > 0)
         motor->SetTargetSpeed(rightSpeed);
 
-      Serial.printf("controlled motor %d, speed = %f\n", motorIx, motor->GetActualSpeed());
+      // Serial.printf("controlled motor %d, speed = %f\n", motorIx,
+      //               motor->GetActualSpeed());
     }
   };
 }
@@ -148,6 +157,13 @@ Quadcopter* Propulsion::GetQuadcopter() {
   return quadcopter;
 }
 
+/// @brief Odometer returns the total distance traveled since start
+/// @return The total distance
+/// This returns the average distance of all wheels. The odometer cannot be
+/// reset. When using a non-directional encoder, the distance is always
+/// increasing. When using a directional encoder, the distance may go down when
+/// the robot is driving backward.
+/// When no wheel encoder is present, this function always returns zero.
 float Propulsion::GetOdometer() {
   float odometer = 0;
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
@@ -173,13 +189,15 @@ bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
     Serial.printf("Odometer = %f\n", distance);
     if (distance >= this->targetDistance) {
       this->driving = false;
-      return true;
+      point = Vector3::zero;
+      rotation = 0;
     }
   } else {
     double duration = difftime(time(NULL), this->startTime);
     if (duration >= this->targetDistance) {
       this->driving = false;
-      return true;
+      point = Vector3::zero;
+      rotation = 0;
     }
   }
 
@@ -187,9 +205,9 @@ bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
     rotation = 1;
   if (rotation < 0)
     rotation = -1;
-  SetTwistSpeed(point.normalized(), rotation);
+  SetTwistSpeed(point.normalized() * this->maxSpeed, rotation);
 
   Update(currentTimeMs);
 
-  return false;
+  return (!this->driving);
 }
\ No newline at end of file
diff --git a/Propulsion.h b/Propulsion.h
index 53e2f68..06edc39 100644
--- a/Propulsion.h
+++ b/Propulsion.h
@@ -23,6 +23,14 @@ class Propulsion {
   unsigned int GetMotorCount();
   Motor* GetMotor(unsigned int motorIx);
 
+  /// @brief Set the maximum linear speed.
+  /// @param maxSpeed The new maximum linear speed
+  /// For controlled motors, this is rpm.
+  /// For calibrated controlled motors, this is m/s
+  /// For uncontrolled motors this is a value between 0 and 1 where 1 is the
+  /// maximum speed of the motor
+  void SetMaxSpeed(float maxSpeed);
+
   // Velocity control
   void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
   void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
@@ -48,6 +56,8 @@ class Propulsion {
  protected:
   Quadcopter* quadcopter = nullptr;
 
+  float maxSpeed = 1;
+
   bool driving = false;
   float targetDistance = 0;
   bool hasOdometer = false;
diff --git a/Roboid.cpp b/Roboid.cpp
index 8e33ec0..5976b00 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -34,11 +34,11 @@ void Roboid::Update(float currentTimeMs) {
                 waypoint->point.z, waypoint->rotation);
   if (Drive(waypoint, currentTimeMs)) {
     this->waypointIx++;
-    if (this->waypointIx == this->trajectory->waypointCount) {
+    if (this->waypointIx >= this->trajectory->waypointCount) {
       this->trajectory = nullptr;
       this->waypointIx = 0;
       // stop
-      propulsion.SetTwistSpeed(0, 0);
+      // propulsion.SetTwistSpeed(0, 0);
     }
   }
 }
\ No newline at end of file