diff --git a/data_types.h b/data_types.h
index bcb021d..3359c81 100644
--- a/data_types.h
+++ b/data_types.h
@@ -1,7 +1,7 @@
 #ifndef DataTypes_h
 #define DataTypes_h
 
-const int LOG_LEVEL_SILENT = 1;
+const int LOG_LEVEL_SILENT = 0;
 const int LOG_LEVEL_WARN = 1;
 const int LOG_LEVEL_INFO = 2;
 const int LOG_LEVEL_DEBUG = 3;
diff --git a/lisa-arduino.ino b/lisa-arduino.ino
index 3740ecc..34e3de1 100644
--- a/lisa-arduino.ino
+++ b/lisa-arduino.ino
@@ -6,7 +6,9 @@
 // *** DEFINE COURSE HERE ***
 const long EMERGENCY_TIMEOUT_MS = 1 * 60 * 1000;
 Waypoint waypoints[] = {
+  //Waypoint{10, 0, 0.3},
   // Left 10m square
+  /*
   Waypoint{8, 0, 0.3},
   Waypoint{10, 2, 0.3},
   Waypoint{10, 8, 0.3},
@@ -14,6 +16,7 @@ Waypoint waypoints[] = {
   Waypoint{2, 10, 0.3},
   Waypoint{0, 8, 0.3},
   Waypoint{0, -3, 3},
+  */
 
   // Right 10m square
   /*
@@ -27,7 +30,6 @@ Waypoint waypoints[] = {
   */
 
   // Figure 8
-  /*
   Waypoint{3, -3, 0.3},
   Waypoint{6, 0, 0.3},
   Waypoint{9, 3, 0.3},
@@ -37,7 +39,6 @@ Waypoint waypoints[] = {
   Waypoint{3, 3, 0.3},
   Waypoint{0, 0, 0.3},
   Waypoint{-4, 0, 3},
-  */
 };
 
 // PIN ASSIGNMENTS
@@ -49,14 +50,14 @@ const int WHEEL_ENCODER_INT = 0; // pin 2
 
 // TODO: Move into a class
 const long MOCK_WHEEL_ENCODER_PIN = 8;
-const bool MOCK_IMU = true;
+const bool MOCK_IMU = false;
 void handleMockSensors();
 
 // Create helper objects
-PositionTracker tracker(LOG_LEVEL_INFO);
-WaypointManager manager(LOG_LEVEL_INFO);
-Navigator navigator(LOG_LEVEL_DEBUG);
-int logLevel = LOG_LEVEL_INFO;
+PositionTracker tracker(LOG_LEVEL_SILENT);
+WaypointManager manager(LOG_LEVEL_SILENT);
+Navigator navigator(LOG_LEVEL_SILENT);
+int logLevel = LOG_LEVEL_SILENT;
 
 // Global values for wheel encoder and IMU
 volatile long gWheelEncoderTicks = 0;
@@ -140,16 +141,19 @@ void loop() {
 }
 
 // If IMU data available read it
-// TODO: Consider moving this to PositionTracker and just have it read and wait for data
 void serialEvent() {
   if (!MOCK_IMU) {
-    gRoll = Serial.parseFloat();
-    gPitch = Serial.parseFloat();
-    gYaw = Serial.parseFloat();
+    float roll = Serial.parseFloat();
+    float pitch = Serial.parseFloat();
+    float yaw = Serial.parseFloat();
     char garbage[20];
     Serial.readBytesUntil('\0', garbage, 20);
     //Serial.print("gYaw: ");
     //Serial.println(gYaw);
+    // NOTE: Sometimes the IMU spikes changes so limit the size we believe for a given fast loop rad
+    //if (abs(yaw - gYaw) < IMU_MAX_DELTA_DEGREES) {
+      gYaw = yaw;
+    //}
   }
 }
 
diff --git a/position_tracker.cpp b/position_tracker.cpp
index 6373790..8a1688c 100644
--- a/position_tracker.cpp
+++ b/position_tracker.cpp
@@ -24,13 +24,6 @@ Position PositionTracker::update() {
   //Serial.print(gYaw);
   //Serial.print(", rDelta: ");
   //Serial.println(rDelta);
-  // NOTE: Sometimes the IMU spikes changes so limit the size we believe
-  if (abs(rDelta) > IMU_MAX_DELTA_RADIANS) {
-    // Skip this position update and get next time
-    // TODO: Return an error code so we can blink it out
-    return this->position;
-  }
-  // Successful read, store as last read
   this->lastYaw = gYaw;
 
   // Figure out wheel encoder delta, update stored value and calculate distance
diff --git a/position_tracker.h b/position_tracker.h
index 525d141..e967ca5 100644
--- a/position_tracker.h
+++ b/position_tracker.h
@@ -10,7 +10,7 @@ extern volatile float gPitch;
 extern volatile float gRoll;
 extern volatile float gYaw;
 
-const float IMU_MAX_DELTA_RADIANS = 10.0 * PI / 180.0;
+const float IMU_MAX_DELTA_DEGREES = 10.0;
 
 // TODO: Put the actual things measured in here and use to compute M so that it's documented
 const float WHEEL_ENCODER_M_DISTANCE_FROM_TICKS = 0.0544737;
diff --git a/speed_calibration.h b/speed_calibration.h
index 4bedcae..0305bb5 100644
--- a/speed_calibration.h
+++ b/speed_calibration.h
@@ -9,7 +9,7 @@ const float SPEED_B_SERVO_FROM_VELOCITY = 659.19;
 
 const int SPEED_STOPPED_SERVO = 0;
 // NOTE: We can directly set velocity and servo instead of curve evaluation if we like
-const float SPEED_LOW_VELOCITY = 0.5;
+const float SPEED_LOW_VELOCITY = 1.0;
 const int SPEED_LOW_SERVO = max(min(int(SPEED_M_SERVO_FROM_VELOCITY * SPEED_LOW_VELOCITY + SPEED_B_SERVO_FROM_VELOCITY), SPEED_MAX_SERVO), SPEED_MIN_SERVO);
 // NOTE: If you bump up the top speed you should also make it do updates in the main loop faster or allow more IMU angle variation, otherwise we turn to fast to track
 const float SPEED_HIGH_VELOCITY = 2.0;
diff --git a/steering_calibration.h b/steering_calibration.h
index 99e2eab..c8a9543 100644
--- a/steering_calibration.h
+++ b/steering_calibration.h
@@ -14,18 +14,18 @@ const float INCH_TO_M = 0.0254;
 // and pick to either turn 0, fixed left or fixed right, where we have the
 // direct correlation to left and right determined already.
 const float STEERING_CENTER_SERVO = 1608; // Not (STEERING_MAX_SERVO + STEERING_MIN_SERVO) / 2 by experimentation
-//const float STEERING_LEFT_SERVO = 1864; // half to max
-//const float STEERING_LEFT_TURN_RADIUS_INCH = 146;
-//const float STEERING_LEFT_TURN_RADIUS = STEERING_LEFT_TURN_RADIUS_INCH * INCH_TO_M; // m
-const float STEERING_LEFT_SERVO = 2120; // max
-const float STEERING_LEFT_TURN_RADIUS_INCH = 77;
+const float STEERING_LEFT_SERVO = 1864; // half to max
+const float STEERING_LEFT_TURN_RADIUS_INCH = 146;
 const float STEERING_LEFT_TURN_RADIUS = STEERING_LEFT_TURN_RADIUS_INCH * INCH_TO_M; // m
-//const float STEERING_RIGHT_SERVO = 1379; // picked half way to max
-//const float STEERING_RIGHT_TURN_RADIUS_INCH = -116;
-//const float STEERING_RIGHT_TURN_RADIUS = STEERING_RIGHT_TURN_RADIUS_INCH * INCH_TO_M; // m
-const float STEERING_RIGHT_SERVO = 1150; // picked half way to max
-const float STEERING_RIGHT_TURN_RADIUS_INCH = -66;
+//const float STEERING_LEFT_SERVO = 2120; // max
+//const float STEERING_LEFT_TURN_RADIUS_INCH = 77;
+//const float STEERING_LEFT_TURN_RADIUS = STEERING_LEFT_TURN_RADIUS_INCH * INCH_TO_M; // m
+const float STEERING_RIGHT_SERVO = 1379; // picked half way to max
+const float STEERING_RIGHT_TURN_RADIUS_INCH = -116;
 const float STEERING_RIGHT_TURN_RADIUS = STEERING_RIGHT_TURN_RADIUS_INCH * INCH_TO_M; // m
+//const float STEERING_RIGHT_SERVO = 1150; // picked half way to max
+//const float STEERING_RIGHT_TURN_RADIUS_INCH = -66;
+//const float STEERING_RIGHT_TURN_RADIUS = STEERING_RIGHT_TURN_RADIUS_INCH * INCH_TO_M; // m
 
 //// Pick our 30 and 10 degree turn radiuses based on what we can hit with both left and right to standardize
 //const float TURN_RADIUS_FOR_30_DEGREES = 1.0; // m