diff --git a/include/Vehicle.h b/include/Vehicle.h
index 3813c90..9770c4a 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -61,13 +61,13 @@ void init_Vehicle(Vehicle &State) {
   State.vz = 0.0; // [m/s]
 
   // Initial YPR
-  State.yaw = 45.0 * M_PI / 180.0;   // [rad]
-  State.pitch = 45.0 * M_PI / 180.0; // [rad]
+  State.yaw = 0.0 * M_PI / 180.0;   // [rad]
+  State.pitch = 0.0 * M_PI / 180.0; // [rad]
   State.roll = 0.0 * M_PI / 180.0;   // [rad]
 
   // Initial YPRdot
-  State.yawdot = 1.0 * M_PI / 180.0;    // [rad/s]
-  State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s]
+  State.yawdot = 0.0 * M_PI / 180.0;    // [rad/s]
+  State.pitchdot = 0.0 * M_PI / 180.0; // [rad/s]
   State.rolldot = 0.0 * M_PI / 180.0;   // [rad/s]
 
   // Servo Limitation
diff --git a/include/teensy.h b/include/teensy.h
index 94d76e0..285069b 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -55,8 +55,9 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   yaw.write(yaw_conv(servoTest));
   pitch.write(pitch_conv(servoTest));
 
+  delay(1000);
+
   // Servo 1 Test
-  Serial.println("1");
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
     yaw.write(yaw_conv(servoTest));
     delay(30);
@@ -69,7 +70,6 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   delay(1000);
 
   // Servo 2 Test
-  Serial.println("2");
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
     pitch.write(pitch_conv(servoTest));
     delay(30);
@@ -82,7 +82,6 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   delay(1000);
 
   // Servo 1 & 2 Test
-  Serial.println("1 and 2");
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
     yaw.write(yaw_conv(servoTest));
     pitch.write(pitch_conv(servoTest));