diff --git a/src/main.cpp b/src/main.cpp
index 9304af1..60ae7e0 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -14,15 +14,16 @@ float calc_height(float yaw, float pitch);
 float yaw_conv(float theta);
 float pitch_conv(float theta);
 
-void setup() {
+void setup()
+{
   delay(5000);
   yaw.attach(29);   // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
   pitch.attach(33); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
 
   Serial.println("Remove Servo Horns and press enter.");
   read_float();
-  yaw.write(yaw_conv(0));
-  pitch.write(pitch_conv(0));
+  yaw.write(yaw_conv(90));
+  pitch.write(pitch_conv(90));
 
   Serial.println("Replace horns, level the TVC and press enter.");
   read_float();
@@ -30,14 +31,18 @@ void setup() {
   Serial.println("Testing Servos");
 
   // Do a full range of movement of the servos
-  for (double pct = -100.0; pct <= 100.0; pct += 10.0) {
+  for (double pct = -100.0; pct <= 100.0; pct += 10.0)
+  {
     Serial.print(".");
-    double yaw_pos = pct * YAW_MAX;
-    double pitch_pos = pct * PITCH_MAX;
+    double yaw_pos = (pct / 100) * YAW_MAX;
+    double pitch_pos = (pct / 100) * PITCH_MAX;
 
-    yaw.write(yaw_conv(yaw_pos));
-    pitch.write(pitch_conv(pitch_pos));
-    delay(100);
+    Serial.println(90 + yaw_conv(yaw_pos));
+    Serial.println(90 + pitch_conv(pitch_pos));
+
+    yaw.write(90 + yaw_conv(yaw_pos));
+    pitch.write(90 + pitch_conv(pitch_pos));
+    delay(500);
   }
 
   Serial.print("Please input the point height in millimeters: ");
@@ -45,10 +50,12 @@ void setup() {
   delay(1000);
 }
 
-void loop() {
+void loop()
+{
   Serial.print("Enter Yaw Coord: ");
   float yaw_pos = read_float();
-  if (abs(yaw_pos) > YAW_MAX) {
+  if (abs(yaw_pos) > YAW_MAX)
+  {
     Serial.println("Value too large, defaulting to maximum deflection");
     yaw_pos = YAW_MAX;
   }
@@ -57,7 +64,8 @@ void loop() {
 
   Serial.print("Enter Pitch Coord: ");
   float pitch_pos = read_float();
-  if (abs(pitch_pos) > PITCH_MAX) {
+  if (abs(pitch_pos) > PITCH_MAX)
+  {
     Serial.println("Value too large, defaulting to maximum deflection");
     pitch_pos = PITCH_MAX;
   }
@@ -71,7 +79,8 @@ void loop() {
   pitch.write(pitch_conv(pitch_pos));
 
   Serial.print("Calculating current height");
-  for (int i = 0; i < 5; i++) {
+  for (int i = 0; i < 5; i++)
+  {
     Serial.print(".");
     delay(i * 100);
   }
@@ -85,38 +94,43 @@ void loop() {
   read_float();
 }
 
-float read_float() {
+float read_float()
+{
   char line[80];
   int count = 0;
 
-  while (1) {
-    if (Serial.available() > 0) {
+  while (1)
+  {
+    if (Serial.available() > 0)
+    {
       line[count] = (char)Serial.read(); // store the char
       Serial.print(line[count]);
-      if (line[count++] == '\r') { // if its a CR,
-        line[count] = '\0';        //  zero-terminate it
+      if (line[count++] == '\r')
+      {                     // if its a CR,
+        line[count] = '\0'; //  zero-terminate it
         return String(line).toFloat();
       }
     }
   }
 }
 
-float yaw_conv(float thetad) {
-  Serial.println(thetad);
+float yaw_conv(float thetad)
+{
+  //Serial.println(thetad);
   float h = .2875;
   float H = 1.5;
 
   float theta = thetad * 3.14 / 180;
 
-  return 2 * asin((H * sin(theta / 2)) / h);
+  return (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
 }
-float pitch_conv(float thetad) {
-  Serial.println(thetad);
+float pitch_conv(float thetad)
+{
+  //Serial.println(thetad);
   float h = .2875;
   float H = 1.6;
 
   float theta = thetad * 3.14 / 180;
 
-  Serial.println(2 * asin((H * sin(theta / 2)) / h));
-  return 2 * asin((H * sin(theta / 2)) / h);
+  return (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / 3.14;
 }