diff --git a/Accelerometer.h b/Accelerometer.h
new file mode 100644
index 0000000..44addc8
--- /dev/null
+++ b/Accelerometer.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "Sensor.h"
+
+/// @brief A Sensor which can measure the magnetic field
+class Accelerometer : public Sensor {
+ public:
+  Accelerometer(){};
+
+  /// @brief Returns the direction of the acceleration in the horizontal plane
+  /// @return The direction of the acceleration, negative is left, positive is
+  /// right
+  /// @note The horizontal plane is defined as being orthogonal to the gravity
+  /// vector
+  /// @note the units (degrees, radians, -1..1, ...) depend on the sensor
+  float GetHorizontalAccelerationDirection();
+  /// @brief Returns the magnitude of the acceleration in the horizontal plane
+  /// @return The magnitude. This value is never negative.
+  /// @note the unity (m/s^2, 0..1) depends on the sensor.
+  float GetHorizontalAccelerationMagnitude();
+
+  /// @brief This gives the gravity direciton relative to the down direction.
+  /// @param horizontal the horizontal direction, negative is left, positive is
+  /// right
+  /// @param vertical the vertical direction, negative is down, positive is up
+  /// @note The horizontal plane is defined as being orthogonal to the gravity
+  /// vector
+  /// @note the units (degrees, radians, -1..1, ...) depend on the sensor
+  void GetAccelerationDirection(float* horizontal, float* vertical);
+  /// @brief The magnitude of the gravity field.
+  /// @return The magnitude. This value is never negative.
+  /// @note the unity (m/s^2, 0..1) depends on the sensor.
+  float GetAccelerationMagnitude();
+};
\ No newline at end of file
diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp
index f8145d4..5396c6e 100644
--- a/ControlledMotor.cpp
+++ b/ControlledMotor.cpp
@@ -1,20 +1,45 @@
 #include "ControlledMotor.h"
+#include <Arduino.h>
+ControlledMotor::ControlledMotor() {
+  this->type = Type::ControlledMotor;
+}
 
-ControlledMotor::ControlledMotor() {}
-
-ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) {
+ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
+    : ControlledMotor() {
   this->motor = motor;
   this->encoder = encoder;
 }
 
-void ControlledMotor::SetTargetVelocity(float velocity) {
+void ControlledMotor::SetTargetSpeed(float velocity) {
   this->targetVelocity = velocity;
+  this->rotationDirection = (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
 }
 
-void ControlledMotor::Update(float timeStep) {
-  float velocity = GetActualVelocity();
+void ControlledMotor::Update(float currentTimeMs) {
+  actualVelocity = (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
   float error = targetVelocity - velocity;
 
-  float acceleration = error * timeStep * pidP;    // Just P is used at this moment
+  float timeStep = currentTimeMs - lastUpdateTime;
+  float acceleration =
+      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 a283fce..d60c948 100644
--- a/ControlledMotor.h
+++ b/ControlledMotor.h
@@ -3,6 +3,9 @@
 #include "Encoder.h"
 #include "Motor.h"
 
+/// @brief A motor with speed control
+/// It uses a feedback loop from an encoder to regulate the speed
+/// The speed is measured in revolutions per second.
 class ControlledMotor : public Thing {
  public:
   ControlledMotor();
@@ -14,19 +17,34 @@ class ControlledMotor : public Thing {
   float pidD = 0;
   float pidI = 0;
 
-  void Update(float timeStep);
+  void Update(float currentTimeMs);
 
-  void SetTargetVelocity(float rotationsPerSecond);
-  float GetActualVelocity() {
-    return (int)rotationDirection * encoder->GetRotationsPerSecond();
-  }  // in rotations per second
+  /// @brief Set the target speed for the motor controller
+  /// @param speed the target in revolutions per second.
+  void SetTargetSpeed(float speed);
+
+  /// @brief Get the actual speed from the encoder
+  /// @return The speed in revolutions per second
+  float GetActualSpeed();
+
+  bool Drive(float distance);
 
- protected:
-  float targetVelocity;
   Motor* motor;
   Encoder* encoder;
+
+ protected:
+  float lastUpdateTime;
+  float targetVelocity;
+  float actualVelocity;
+  float netDistance = 0;
+  float startDistance = 0;
+
   enum Direction { Forward = 1,
                    Reverse = -1 };
 
   Direction rotationDirection;
+
+  bool driving = false;
+  float targetDistance = 0;
+  float lastEncoderPosition = 0;
 };
\ No newline at end of file
diff --git a/Encoder.cpp b/Encoder.cpp
index 9b3aba8..652b0f6 100644
--- a/Encoder.cpp
+++ b/Encoder.cpp
@@ -1,44 +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++;
+int Encoder::GetPulseCount() {
+  return 0;
 }
 
-float Encoder::GetRotationsPerSecond() {
-  return rps;
+float Encoder::GetPulsesPerSecond(float currentTimeMs) {
+  return 0;
 }
 
-void Encoder::ResetDistance() {
-  distance = 0;
+float Encoder::GetDistance() {
+  return 0;
 }
 
-float Encoder::GetRotationDistance() {
-  return distance;
+float Encoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
 }
-
-void Encoder::Update(float timeStep) {
-  // Hmmm. Arduino-dependent code...
-  // noInterrupts();
-  float distanceThisUpdate = transitionCount / transitionsPerRotation;
-  transitionCount = 0;
-  // interrupts();
-
-  // float rps = distanceThisUpdate * timeStep;
-  distance += distanceThisUpdate;
-}
\ No newline at end of file
diff --git a/Encoder.h b/Encoder.h
index e734139..4573901 100644
--- a/Encoder.h
+++ b/Encoder.h
@@ -3,21 +3,14 @@
 class Encoder {
  public:
   Encoder();
-  Encoder(unsigned char pin, unsigned char transitionsPerRotation);
+  Encoder(unsigned char transitionsPerRotation);
 
-  float GetRotationsPerSecond();
+  virtual int GetPulseCount();
+  virtual float GetPulsesPerSecond(float currentTimeMs);
 
-  void ResetDistance();
-  float GetRotationDistance();
-
-  void Update(float timeStep);
+  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/Magnetometer.h b/Magnetometer.h
new file mode 100644
index 0000000..ee9cf88
--- /dev/null
+++ b/Magnetometer.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "Sensor.h"
+
+/// @brief A Sensor which can measure the magnetic field
+class Magnetometer : public Sensor {
+ public:
+  Magnetometer();
+
+  /// @brief Returns the direction of the magnetic field relative to the forward direction
+  /// @return The direction, negative is to the left, positive is to the right
+  /// @note The actual unit (degrees, radians, -1..1, ...) depends on the sensor.
+  virtual float GetDirection();
+  /// @brief Returns the inclination of the magnetic field relative to the horizontal plane
+  /// @return The direction, negative is downward, positive is upward
+  /// @note The actual unit (degrees, radias, -1..1, ...) depends on the sensor.
+  virtual float GetInclination();
+
+  /// @brief Returns the strength of the magnetic field
+  /// @return The strength. This values should always be positive
+  /// @note The actual unit (tesla, 0..1, ...) depends on the sensor.
+  virtual unsigned float GetMagnitude();
+}
\ No newline at end of file
diff --git a/Motor.cpp b/Motor.cpp
index 7842cfa..76f8646 100644
--- a/Motor.cpp
+++ b/Motor.cpp
@@ -1,33 +1,32 @@
 #include "Motor.h"
+#include <time.h>
+
+#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;
+  return this->currentSpeed;
 }
 
 void Motor::SetSpeed(float speed) {
-	this->currentSpeed = speed;
+  this->currentSpeed = speed;
+}
+
+bool Motor::Drive(float distance) {
+  if (!this->driving) {
+    this->startTime = time(NULL);
+    this->targetDistance = abs(distance);
+    this->driving = true;
+  }
+  double duration = difftime(time(NULL), this->startTime);
+  if (duration >= this->targetDistance) {
+    this->driving = false;
+    return true;
+  }
+
+  SetSpeed(distance < 0 ? -1 : 1);  // max speed
+  return false;
 }
\ No newline at end of file
diff --git a/Motor.h b/Motor.h
index 921171d..e42fb05 100644
--- a/Motor.h
+++ b/Motor.h
@@ -1,13 +1,13 @@
 #pragma once
 
+#include <time.h>
 #include "Thing.h"
 
 class Motor : public Thing {
  public:
   Motor();
   /// @brief Turning direction of the motor
-  enum Direction { Forward = 1,
-                   Reverse = -1 };
+  enum Direction { Forward = 1, Reverse = -1 };
 
   /// @brief Set the turning direction of the motor
   // void SetDirection(Direction direction);
@@ -15,6 +15,11 @@ class Motor : public Thing {
   virtual void SetSpeed(float speed);
   float GetSpeed();
 
+  bool Drive(float distance);
+
  protected:
   float currentSpeed = 0;
+  bool driving = false;
+  float targetDistance = 0;
+  time_t startTime = 0;
 };
diff --git a/Propulsion.cpp b/Propulsion.cpp
index 775ee88..463e03a 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -3,31 +3,28 @@
 
 #include "FloatSingle.h"
 
+#include <Arduino.h>
+
 Propulsion::Propulsion() {
   this->placement = nullptr;
   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 || 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 || thing->type == Thing::Type::ControlledMotor)
       this->placement[motorIx++] = things[thingIx];
   }
 }
@@ -45,39 +42,60 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
     return nullptr;
 
   Thing* thing = this->placement[motorId].thing;
-  if (thing->isMotor)
+  // if (thing->isMotor)
+  if (thing->type == Thing::Type::Motor)
     return (Motor*)thing;
 
   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());
+    }
   };
 }
 
-void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
+void Propulsion::SetDiffDriveVelocities(float leftVelocity,
+                                        float rightVelocity) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     // Placement placement = placement[motorIx];
     // if (placement.position.x < 0)
@@ -88,7 +106,8 @@ void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity)
 }
 
 void Propulsion::SetTwistSpeed(float forward, float yaw) {
-  // This is configuration dependent, a drone will do something completely different...
+  // This is configuration dependent, a drone will do something completely
+  // different...
   float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
   float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
   SetDiffDriveSpeed(leftSpeed, rightSpeed);
@@ -107,19 +126,70 @@ 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, float turningVelocity) {
+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) {
+void Propulsion::SetLinearSpeed(Vector3 velocity,
+                                float yawSpeed,
+                                float rollSpeed) {
   if (quadcopter != nullptr)
     quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
 }
 
 Quadcopter* Propulsion::GetQuadcopter() {
   return quadcopter;
+}
+
+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;
+  }
+  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)
+    rotation = 1;
+  if (rotation < 0)
+    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 3d6c70f..dad7296 100644
--- a/Propulsion.h
+++ b/Propulsion.h
@@ -1,32 +1,26 @@
 #pragma once
-
-#include "Quadcopter.h"
 #include "ControlledMotor.h"
 #include "Placement.h"
+#include "Quadcopter.h"
 #include "Vector2.h"
 
-#include <list>
-
-//struct MotorPlacement {
-//  Motor* motor;
-//  ControlledMotor* controlledMotor;
-//  Vector2 position;
-//};
+#include <time.h>
 
 class Propulsion {
  public:
   /// @brief Setup sensing
   Propulsion();
 
-  void Update();
+  void Update(float currentTimeMs);
 
-  //void AddMotors(MotorPlacement* motors, unsigned int motorCount);
   void AddMotors(Placement* motors, unsigned int motorCount);
   void AddQuadcopter(Quadcopter* quadcopter);
+  Quadcopter* GetQuadcopter();
 
   unsigned int GetMotorCount();
   Motor* GetMotor(unsigned int motorIx);
 
+  // Velocity control
   void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
   void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
 
@@ -36,14 +30,25 @@ class Propulsion {
   void SetTwistVelocity(float forward, float yaw);
 
   // Think: drones
-  Quadcopter* GetQuadcopter();
-  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F, float rollSpeed = 0.0F);
+  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);
+
+  float GetOdometer();
 
- protected:
-  //unsigned long lastMillis;
-  //MotorPlacement* motors = nullptr;
   Placement* placement = nullptr;
   unsigned int motorCount = 0;
 
+ protected:
   Quadcopter* quadcopter = nullptr;
+
+  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 b0cca1e..7bf37ef 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -1,5 +1,5 @@
 #include "Roboid.h"
-
+#include <Arduino.h>
 Roboid::Roboid() {
   this->configuration = nullptr;
   this->thingCount = 0;
@@ -11,4 +11,32 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
 
   perception.AddSensors(configuration, thingCount);
   propulsion.AddMotors(configuration, thingCount);
+}
+
+bool Roboid::Drive(Waypoint* waypoint, float currentTimeMs) {
+  bool finished = propulsion.Drive(waypoint->point, waypoint->rotation, currentTimeMs);
+  return finished;
+}
+
+void Roboid::FollowTrajectory(Trajectory* trajectory) {
+  this->trajectory = trajectory;
+  this->waypointIx = 0;
+}
+
+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, currentTimeMs)) {
+    this->waypointIx++;
+    if (this->waypointIx == this->trajectory->waypointCount) {
+      this->trajectory = nullptr;
+      this->waypointIx = 0;
+      // stop
+      propulsion.SetTwistSpeed(0, 0);
+    }
+  }
 }
\ No newline at end of file
diff --git a/Roboid.h b/Roboid.h
index 7aa1b23..3b10ec4 100644
--- a/Roboid.h
+++ b/Roboid.h
@@ -5,6 +5,34 @@
 #include "Placement.h"
 #include "Propulsion.h"
 
+class Waypoint {
+ public:
+  Waypoint(float forwardDistance, float rotation) {
+    this->point = Vector3(0, 0, forwardDistance);
+    this->distance = forwardDistance;
+    this->rotation = rotation;
+  }
+  float distance = 0;
+  Vector3 point = Vector3(0, 0, 0);
+  float rotation = 0;
+};
+class Trajectory {
+ public:
+  Trajectory(){};
+  Trajectory(Waypoint* waypoints, unsigned int waypointCount) {
+    this->waypoints = waypoints;
+    this->waypointCount = waypointCount;
+  }
+
+  Waypoint* waypoints;
+  unsigned int waypointCount;
+};
+
+class Acceleration {
+ public:
+  float GetMagnitude() { return 0; };
+};
+
 class Roboid {
  public:
   Roboid();
@@ -12,7 +40,17 @@ class Roboid {
 
   Perception perception;
   Propulsion propulsion;
+  Acceleration acceleration;
 
   Placement* configuration;
   unsigned int thingCount;
+
+  void Update(float currentTimeMs);
+
+  bool Drive(Waypoint* waypoint, float currentTimeMs);
+  void FollowTrajectory(Trajectory* Trajectory);
+
+ public:
+  Trajectory* trajectory;
+  unsigned int waypointIx = 0;
 };
\ No newline at end of file
diff --git a/Sensing.cpp b/Sensing.cpp
index 84ec2c2..0a214ac 100644
--- a/Sensing.cpp
+++ b/Sensing.cpp
@@ -29,7 +29,8 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
   sensorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    if (thing->isSensor)
+    // if (thing->isSensor)
+    if (thing->type == Thing::Type::Sensor)
       sensorCount++;
   }
 
@@ -38,7 +39,8 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
   unsigned int sensorIx = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    if (thing->isSensor)
+    // if (thing->isSensor)
+    if (thing->type == Thing::Type::Sensor)
       sensorPlacements[sensorIx++] = things[thingIx];
   }
 }
@@ -52,7 +54,8 @@ Sensor* Sensing::GetSensor(unsigned int sensorId) {
     return nullptr;
 
   Thing* thing = this->sensorPlacements[sensorId].thing;
-  if (thing->isSensor)
+  // if (thing->isSensor)
+  if (thing->type == Thing::Type::Sensor)
     return (Sensor*)thing;
 
   return nullptr;
@@ -167,7 +170,9 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) {
 }
 
 unsigned int Sensing::ToDepthMapIndex(float angle) {
-  unsigned int depthMapIx = (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * (float)resolution);
+  unsigned int depthMapIx =
+      (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) *
+                     (float)resolution);
   return depthMapIx;
 }
 
diff --git a/Sensor.cpp b/Sensor.cpp
index c6511d3..e4ee22d 100644
--- a/Sensor.cpp
+++ b/Sensor.cpp
@@ -1,5 +1,6 @@
 #include "Sensor.h"
 
 Sensor::Sensor() {
-  this->isSensor = true;
+  // this->isSensor = true;
+  type = Type::Sensor;
 }
\ No newline at end of file
diff --git a/Sensor.h b/Sensor.h
index f49bfe6..1f47259 100644
--- a/Sensor.h
+++ b/Sensor.h
@@ -2,6 +2,7 @@
 
 #include "Thing.h"
 
+/// @brief A sensor is a thing which can perform measurements in the environment
 class Sensor : public Thing {
  public:
   Sensor();
diff --git a/Thing.h b/Thing.h
index 610542c..8e0561d 100644
--- a/Thing.h
+++ b/Thing.h
@@ -1,8 +1,16 @@
 #pragma once
 
+/// @brief A thing is a functional component on a robot
 class Thing {
-public:
-	Thing() { isSensor = false, isMotor = false; }
-	bool isSensor;
-	bool isMotor;
+ public:
+  Thing() {
+    type = Type::Undetermined;
+    // isSensor = false, isMotor = false;
+  }
+
+  enum class Type { Undetermined, Sensor, Motor, ControlledMotor };
+  Type type = Type::Undetermined;
+  // bool isSensor;
+  // bool isMotor;
+  // bool isControlledMotor;
 };
\ No newline at end of file
diff --git a/library.json b/library.json
new file mode 100644
index 0000000..8b014b3
--- /dev/null
+++ b/library.json
@@ -0,0 +1,11 @@
+{
+    "name": "RoboidControl",
+    "version": "0.0",
+    "build": {
+        "srcDir": ".",
+        "flags": [
+            "-I .",
+            "-I ./VectorAlgebra/include"
+        ]
+    }
+}
\ No newline at end of file