diff --git a/include/Vehicle.h b/include/Vehicle.h
index ac8b24c..ed47313 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -80,7 +80,7 @@ void init_Vehicle(Vehicle &State) {
   State.momentArm = 0.145;       // [m]
 
   // Sim Step Size
-  State.stepSize = 20.0; // [ms]
+  State.stepSize = 5.0; // [ms]
 
   // Other Properties
   State.massPropellant = 0.06;                                  // [kg]
diff --git a/include/teensy.h b/include/teensy.h
index e57e6c0..0ad10f4 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -18,7 +18,6 @@ File dataFile;
 
 void testGimbal(PWMServo &servo1, PWMServo &servo2) {
   int servoTest = 0;
-  ;
 
   servo1.write(servoTest);
   servo2.write(servoTest);
@@ -45,6 +44,20 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
     delay(30);
   }
 
+  delay(1000);
+
+  // Servo 1 & 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(servoTest);
+    servo2.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(servoTest);
+    servo2.write(servoTest);
+    delay(30);
+  }
+
   delay(30);
   servo1.write(0);
   servo2.write(0);
diff --git a/src/main.cpp b/src/main.cpp
index f55f9e4..dbc46fb 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -105,17 +105,18 @@ void loop() {
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     printSimResults(State);
-    Serial.println("Run duration:" + String(micros() - initTime) + " us");
+    Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
+                   " seconds");
 
     closeFile();
     delay(3000);
 
     Serial.println("SUCCESS");
-    Serial.println("Aborting Sim");
+    Serial.println("Exiting Sim");
     teensyAbort();
   }
 
-  delay(20.0 - ((micros() - last) / 1000.0));
+  delay(State.stepSize - ((micros() - last) / 1000.0));
 }
 
 void init_loadCells() {