diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp
index 5396c6e..3462bbc 100644
--- a/ControlledMotor.cpp
+++ b/ControlledMotor.cpp
@@ -1,7 +1,7 @@
 #include "ControlledMotor.h"
 #include <Arduino.h>
 ControlledMotor::ControlledMotor() {
-  this->type = Type::ControlledMotor;
+  this->type = Thing::ControlledMotorType;
 }
 
 ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
@@ -12,22 +12,24 @@ ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
 
 void ControlledMotor::SetTargetSpeed(float velocity) {
   this->targetVelocity = velocity;
-  this->rotationDirection = (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
+  this->rotationDirection =
+      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
 }
 
 void ControlledMotor::Update(float currentTimeMs) {
-  actualVelocity = (int)rotationDirection * encoder->GetRevolutionsPerSecond(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);
+  return actualVelocity;
 }
 
 bool ControlledMotor::Drive(float distance) {
@@ -36,10 +38,7 @@ bool ControlledMotor::Drive(float 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 a732949..66d958f 100644
--- a/ControlledMotor.h
+++ b/ControlledMotor.h
@@ -14,6 +14,9 @@ class ControlledMotor : public Thing {
   ControlledMotor();
   ControlledMotor(Motor* motor, Encoder* encoder);
 
+  inline static bool CheckType(Thing* thing) {
+    return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
+  }
   float velocity;
 
   float pidP = 1;
diff --git a/DistanceSensor.cpp b/DistanceSensor.cpp
index dacf8cb..c1224a2 100644
--- a/DistanceSensor.cpp
+++ b/DistanceSensor.cpp
@@ -1,34 +1,27 @@
 #include "DistanceSensor.h"
 
-DistanceSensor::DistanceSensor()
-{
-    isDistanceSensor = true;
+DistanceSensor::DistanceSensor() {
+  this->type = Thing::DistanceSensorType;
 }
 
-DistanceSensor::DistanceSensor(float triggerDistance)
-{
-    isDistanceSensor = true;
-    this->triggerDistance = triggerDistance;
+DistanceSensor::DistanceSensor(float triggerDistance) {
+  this->triggerDistance = triggerDistance;
 }
 
-float DistanceSensor::GetDistance()
-{
-    return distance;
+float DistanceSensor::GetDistance() {
+  return distance;
 };
 
-void DistanceSensor::SetDistance(float distance)
-{
-    this->distance = distance;
-}; // for simulation purposes
+void DistanceSensor::SetDistance(float distance) {
+  this->distance = distance;
+};  // for simulation purposes
 
-bool DistanceSensor::IsOn()
-{
-    bool isOn = GetDistance() <= triggerDistance;
-    return isOn;
+bool DistanceSensor::IsOn() {
+  bool isOn = GetDistance() <= triggerDistance;
+  return isOn;
 }
 
-bool DistanceSensor::isOff()
-{
-    bool isOff = GetDistance() > triggerDistance;
-    return isOff;
+bool DistanceSensor::isOff() {
+  bool isOff = GetDistance() > triggerDistance;
+  return isOff;
 }
diff --git a/Motor.cpp b/Motor.cpp
index ea6b55d..d083033 100644
--- a/Motor.cpp
+++ b/Motor.cpp
@@ -4,7 +4,7 @@
 // #include <Arduino.h>
 
 Motor::Motor() {
-  type = Type::Motor;
+  type = Thing::MotorType;
 }
 
 float Motor::GetSpeed() {
diff --git a/Propulsion.cpp b/Propulsion.cpp
index 5291985..6a3c5d2 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -14,10 +14,9 @@ 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::MotorType) != 0)
       motorCount++;
-    if (thing->type == Thing::Type::ControlledMotor)
+    if (thing->type == (int)Thing::Type::ControlledMotor)
       hasOdometer = true;
   }
   this->placement = new Placement[motorCount];
@@ -25,8 +24,7 @@ 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::MotorType) != 0)
       this->placement[motorIx++] = things[thingIx];
   }
 }
@@ -44,8 +42,7 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
     return nullptr;
 
   Thing* thing = this->placement[motorId].thing;
-  // if (thing->isMotor)
-  if (thing->type == Thing::Type::Motor)
+  if ((thing->type & Thing::MotorType) != 0)
     return (Motor*)thing;
 
   return nullptr;
@@ -53,13 +50,13 @@ 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;
-    if (thing->type == Thing::Type::ControlledMotor) {
+    if (thing->type == Thing::ControlledMotorType) {
       ControlledMotor* motor = (ControlledMotor*)thing;
       motor->Update(currentTimeMs);
     }
@@ -74,8 +71,8 @@ void Propulsion::SetMaxSpeed(float maxSpeed) {
 void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     Thing* thing = placement[motorIx].thing;
-    if (thing->type == Thing::Type::Motor) {
-      Motor* motor = (Motor*)placement[motorIx].thing;
+    if (thing->type == Thing::UncontrolledMotorType) {
+      Motor* motor = (Motor*)thing;
       if (motor == nullptr)
         continue;
 
@@ -85,8 +82,7 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
       else if (xPosition > 0)
         motor->SetSpeed(rightSpeed);
 
-      Serial.printf("motor %d, speed = %f\n", motorIx, motor->GetSpeed());
-    } else if (thing->type == Thing::Type::ControlledMotor) {
+    } else if (thing->type == Thing::ControlledMotorType) {
       ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing;
       if (motor == nullptr)
         continue;
@@ -96,9 +92,6 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
         motor->SetTargetSpeed(leftSpeed);
       else if (xPosition > 0)
         motor->SetTargetSpeed(rightSpeed);
-
-      // Serial.printf("controlled motor %d, speed = %f\n", motorIx,
-      //               motor->GetActualSpeed());
     }
   };
 }
@@ -168,7 +161,7 @@ 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) {
+    if ((thing->type & Thing::ControlledMotorType) != 0) {
       ControlledMotor* motor = (ControlledMotor*)thing;
       odometer += motor->encoder->GetDistance() / this->motorCount;
     }
@@ -186,7 +179,6 @@ bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
   }
   if (hasOdometer) {
     float distance = GetOdometer() - this->startOdometer;
-    Serial.printf("Odometer = %f\n", distance);
     if (distance >= this->targetDistance) {
       this->driving = false;
       point = Vector3::zero;
diff --git a/Roboid.cpp b/Roboid.cpp
index 5976b00..4561e6d 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -30,8 +30,6 @@ void Roboid::Update(float currentTimeMs) {
     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) {
diff --git a/Sensing.cpp b/Sensing.cpp
index 0a214ac..c55a03a 100644
--- a/Sensing.cpp
+++ b/Sensing.cpp
@@ -1,9 +1,8 @@
 #include "Sensing.h"
 #include "DistanceSensor.h"
-// #include <Switch.h>
+#include "Switch.h"
 
 #include <math.h>
-#include <algorithm>
 
 SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor,
                                  Vector2 direction) {
@@ -19,18 +18,11 @@ SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) {
 
 Sensing::Sensing() {}
 
-// void Sensing::AddSensors(SensorPlacement* sensors, unsigned int sensorCount)
-// {
-//   this->sensors = sensors;
-//   this->sensorCount = sensorCount;
-// }
-
 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->type == Thing::Type::Sensor)
+    if ((thing->type & Thing::SensorType) != 0)
       sensorCount++;
   }
 
@@ -39,9 +31,9 @@ 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->type == Thing::Type::Sensor)
+    if ((thing->type & Thing::SensorType) != 0) {
       sensorPlacements[sensorIx++] = things[thingIx];
+    }
   }
 }
 
@@ -54,8 +46,7 @@ Sensor* Sensing::GetSensor(unsigned int sensorId) {
     return nullptr;
 
   Thing* thing = this->sensorPlacements[sensorId].thing;
-  // if (thing->isSensor)
-  if (thing->type == Thing::Type::Sensor)
+  if (thing->type & Thing::SensorType != 0)
     return (Sensor*)thing;
 
   return nullptr;
@@ -66,7 +57,7 @@ float Sensing::DistanceForward(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->isDistanceSensor == false)
+    if (sensor->type & Thing::SensorType != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -83,7 +74,7 @@ float Sensing::DistanceLeft(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->isDistanceSensor == false)
+    if (sensor->type & Thing::SensorType != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -101,7 +92,7 @@ float Sensing::DistanceRight(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->isDistanceSensor == false)
+    if (sensor->type & Thing::DistanceSensorType != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -119,7 +110,7 @@ float Sensing::DistanceUp(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->isDistanceSensor == false)
+    if (sensor->type & Thing::SensorType != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -136,7 +127,7 @@ float Sensing::DistanceDown(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->isDistanceSensor == false)
+    if (sensor->type & Thing::SensorType != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -154,16 +145,21 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) {
 
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
-    float angle = placement.direction.x;
+    float angle = placement.direction.y;
     if (angle > fromAngle && angle < toAngle) {
-      DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+      Thing* thing = placement.thing;
+      if (thing == nullptr)
+        continue;
 
-      // if (placement.switchSensor != nullptr &&
-      // placement.switchSensor->IsOn())
-      //   return true;
-      // else
-      if (distanceSensor != nullptr && distanceSensor->IsOn())
-        return true;
+      if ((thing->type & (int)Thing::Type::DistanceSensor) != 0) {
+        DistanceSensor* distanceSensor = (DistanceSensor*)thing;
+        if (distanceSensor != nullptr && distanceSensor->IsOn())
+          return true;
+      } else if ((thing->type & (int)Thing::Type::Switch) != 0) {
+        Switch* switchSensor = (Switch*)thing;
+        if (switchSensor != nullptr && switchSensor->IsOn())
+          return true;
+      }
     }
   }
   return false;
diff --git a/Sensing.h b/Sensing.h
index c8d012b..94d56c5 100644
--- a/Sensing.h
+++ b/Sensing.h
@@ -3,7 +3,7 @@
 #include "Placement.h"
 #include "Sensor.h"
 
-#include <list>
+// #include <list>
 
 namespace Passer::RoboidControl {
 
diff --git a/Sensor.cpp b/Sensor.cpp
index e4ee22d..82b20f0 100644
--- a/Sensor.cpp
+++ b/Sensor.cpp
@@ -2,5 +2,5 @@
 
 Sensor::Sensor() {
   // this->isSensor = true;
-  type = Type::Sensor;
+  type = Thing::SensorType;
 }
\ No newline at end of file
diff --git a/Sensor.h b/Sensor.h
index ae81628..4f6f87a 100644
--- a/Sensor.h
+++ b/Sensor.h
@@ -9,7 +9,6 @@ namespace RoboidControl {
 class Sensor : public Thing {
  public:
   Sensor();
-  bool isDistanceSensor = false;
 };
 
 }  // namespace RoboidControl
diff --git a/Thing.h b/Thing.h
index 0c86697..861f218 100644
--- a/Thing.h
+++ b/Thing.h
@@ -6,16 +6,29 @@ namespace RoboidControl {
 /// @brief A thing is a functional component on a robot
 class Thing {
  public:
-  Thing() {
-    type = Type::Undetermined;
-    // isSensor = false, isMotor = false;
-  }
+  Thing() { type = (int)Type::Undetermined; }
 
-  enum class Type { Undetermined, Sensor, Motor, ControlledMotor };
-  Type type = Type::Undetermined;
-  // bool isSensor;
-  // bool isMotor;
-  // bool isControlledMotor;
+  enum class Type {
+    Undetermined,
+    /// Sensor,
+    Switch,
+    DistanceSensor,
+    // Motor,
+    ControlledMotor,
+    UncontrolledMotor,
+  };
+  static const unsigned int MotorType = 0x8000;
+  static const unsigned int SensorType = 0x4000;
+  static const unsigned int SwitchType =
+      SensorType | (unsigned int)Type::Switch;
+  static const unsigned int DistanceSensorType =
+      SensorType | (unsigned int)Type::DistanceSensor;
+  static const unsigned int ControlledMotorType =
+      MotorType | (unsigned int)Type::ControlledMotor;
+  static const unsigned int UncontrolledMotorType =
+      MotorType | (unsigned int)Type::UncontrolledMotor;
+
+  unsigned int type = (int)Type::Undetermined;
 };
 
 }  // namespace RoboidControl
diff --git a/VectorAlgebra b/VectorAlgebra
index 493a3f7..365b177 160000
--- a/VectorAlgebra
+++ b/VectorAlgebra
@@ -1 +1 @@
-Subproject commit 493a3f748907b4fb7e64177f94b7cb98a951af4c
+Subproject commit 365b1773e501566a685a88ebc9334c35814f3ce0