diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp
index bdef4ef..5f331cb 100644
--- a/ControlledMotor.cpp
+++ b/ControlledMotor.cpp
@@ -1,7 +1,8 @@
 #include "ControlledMotor.h"
 
 ControlledMotor::ControlledMotor() {
-  this->isControlledMotor = true;
+  // this->isControlledMotor = true;
+  this->type = Type::ControlledMotor;
 }
 
 ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) {
diff --git a/Motor.cpp b/Motor.cpp
index 7842cfa..a8f8a60 100644
--- a/Motor.cpp
+++ b/Motor.cpp
@@ -1,7 +1,11 @@
 #include "Motor.h"
+#include <time.h>
+
+#include <Arduino.h>
 
 Motor::Motor() {
-  this->isMotor = true;
+  //   this->isMotor = true;
+  type = Type::Motor;
 }
 
 // Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) {
@@ -25,9 +29,25 @@ Motor::Motor() {
 // }
 
 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);
+  return false;
 }
\ No newline at end of file
diff --git a/Motor.h b/Motor.h
index a6a8576..e42fb05 100644
--- a/Motor.h
+++ b/Motor.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <time.h>
 #include "Thing.h"
 
 class Motor : public Thing {
@@ -14,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 e8c30dd..612817e 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -19,7 +19,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->isMotor)
+    // if (thing->isMotor)
+    if (thing->type == Thing::Type::Motor)
       motorCount++;
   }
   this->placement = new Placement[motorCount];
@@ -27,7 +28,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->isMotor)
+    // if (thing->isMotor)
+    if (thing->type == Thing::Type::Motor)
       this->placement[motorIx++] = things[thingIx];
   }
 }
@@ -45,7 +47,8 @@ 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;
diff --git a/Roboid.cpp b/Roboid.cpp
index 6884af3..a3bd8b2 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;
@@ -16,16 +16,51 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
 bool Roboid::Drive(float forwardDistance) {
   bool finished = true;
   for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
-    Motor* motor = (Motor*)propulsion.placement[motorIx].thing;
-    if (motor == nullptr)
-      continue;
-    if (motor->isControlledMotor == false)
-      continue;
+    bool motorFinished = false;
+    Thing* thing = propulsion.placement[motorIx].thing;
 
-    ControlledMotor* controlledMotor = (ControlledMotor*)motor;
-    bool motorFinished = controlledMotor->Drive(forwardDistance);
-    if (motorFinished == false)
+    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;
+    } else
+      Serial.printf("Motor finished\n");
   }
   return finished;
 }
+
+float waypointArray[] = {1, -1};
+void Roboid::FollowTrajectory(Trajectory* Trajectory) {
+  this->waypoints = waypointArray;
+  this->waypointCount = 2;
+  this->waypointIx = 0;
+}
+
+void Roboid::Update() {
+  if (this->waypoints == nullptr)
+    return;
+
+  // Serial.printf("Driving waypoints %d %f\n", this->waypointIx,
+  //               waypoints[waypointIx]);
+  if (Drive(this->waypoints[this->waypointIx])) {
+    this->waypointIx++;
+    if (this->waypointIx == this->waypointCount) {
+      this->waypoints = nullptr;
+      this->waypointIx = 0;
+      this->waypointCount = 0;
+      propulsion.SetTwistSpeed(0, 0);
+    }
+  }
+}
\ No newline at end of file
diff --git a/Roboid.h b/Roboid.h
index a83626d..17af5f3 100644
--- a/Roboid.h
+++ b/Roboid.h
@@ -5,6 +5,15 @@
 #include "Placement.h"
 #include "Propulsion.h"
 
+class WayPoint {
+ public:
+  float distance = 0;
+};
+class Trajectory {
+ public:
+  float* waypoints;
+};
+
 class Acceleration {
  public:
   float GetMagnitude() { return 0; };
@@ -22,5 +31,13 @@ class Roboid {
   Placement* configuration;
   unsigned int thingCount;
 
+  void Update();
+
   bool Drive(float forwardDistance);
+  void FollowTrajectory(Trajectory* Trajectory);
+
+ public:
+  float* waypoints = nullptr;
+  unsigned int waypointIx = 0;
+  unsigned int waypointCount = 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/Thing.h b/Thing.h
index 0351995..8e0561d 100644
--- a/Thing.h
+++ b/Thing.h
@@ -3,8 +3,14 @@
 /// @brief A thing is a functional component on a robot
 class Thing {
  public:
-  Thing() { isSensor = false, isMotor = false; }
-  bool isSensor;
-  bool isMotor;
-  bool isControlledMotor;
+  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