diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp
index 321af55..9e0c2cf 100644
--- a/DifferentialDrive.cpp
+++ b/DifferentialDrive.cpp
@@ -1,4 +1,5 @@
 #include "DifferentialDrive.h"
+#include "VectorAlgebra/Angle.h"
 #include "VectorAlgebra/FloatSingle.h"
 
 #include <math.h>
@@ -22,8 +23,6 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor,
   rightMotor->position.distance = distance;
 }
 
-#include <Arduino.h>
-
 void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     Motor *motor = motors[motorIx];
@@ -53,15 +52,13 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
   SetTwistSpeed(linear.z, yaw);
 }
 
-#include <Arduino.h>
-
 Polar DifferentialDrive::GetVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
-  leftSpeed = leftSpeed * wheelDiameter * M_PI;    // in meters per second
-  rightSpeed = rightSpeed * wheelDiameter * M_PI;  // in meters per second
+  leftSpeed = leftSpeed * wheelDiameter * Angle::PI;   // in meters per second
+  rightSpeed = rightSpeed * wheelDiameter * Angle::PI; // in meters per second
   float speed = (leftSpeed + rightSpeed) / 2;
 
   float direction = speed >= 0 ? 0.0F : 180.0F;
@@ -75,24 +72,14 @@ float DifferentialDrive::GetAngularVelocity() {
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
-  leftSpeed = leftSpeed * wheelDiameter * M_PI;    // in meters per second
-  rightSpeed = rightSpeed * wheelDiameter * M_PI;  // in meters per second
+  leftSpeed = leftSpeed * wheelDiameter * Angle::PI;   // in meters per second
+  rightSpeed = rightSpeed * wheelDiameter * Angle::PI; // in meters per second
 
   float angularSpeed = (rightSpeed - leftSpeed) / 2;
-  float angularDistance = wheelSeparation / 2 * M_PI;
+  float angularDistance = wheelSeparation / 2 * Angle::PI;
   float rotationsPerSecond = angularSpeed / angularDistance;
   float degreesPerSecond = 360 * rotationsPerSecond;
   float angularVelocity = degreesPerSecond;
 
-  // Serial.print(leftSpeed);
-  // Serial.print("  -  ");
-  // Serial.println(rightSpeed);
-
-  // Serial.print(angularSpeed);
-  // Serial.print("  ");
-  // Serial.print(angularDistance);
-  // Serial.print("  ");
-  // Serial.println(rotationsPerSecond);
-
   return angularVelocity;
 }
\ No newline at end of file
diff --git a/VectorAlgebra b/VectorAlgebra
index 96bcc4a..78ed576 160000
--- a/VectorAlgebra
+++ b/VectorAlgebra
@@ -1 +1 @@
-Subproject commit 96bcc4a405e4df95b7f54ba99fcd2c60079e0ae6
+Subproject commit 78ed576e16bbe479209c0b0927378b27d31dd5f4