diff --git a/include/teensy.h b/include/teensy.h
index b42f186..1a97f62 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -14,6 +14,8 @@ void write2CSV(struct Vehicle &);
 void printSimResults(struct Vehicle &);
 void teensyAbort();
 double loadCellFilter(double current, double previous);
+float yaw_conv(float thetad);
+float pitch_conv(float thetad);
 
 // Load cell stuff
 HX711 lc0;
@@ -42,19 +44,19 @@ double lcGain3 = -118.65;
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
-void testGimbal(PWMServo &servo1, PWMServo &servo2) {
+void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   int servoTest = 90;
 
-  servo1.write(servoTest);
-  servo2.write(servoTest);
+  yaw.write(yaw_conv(servoTest));
+  pitch.write(pitch_conv(servoTest));
 
   // Servo 1 Test
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
-    servo1.write(90 + servoTest);
+    yaw.write(yaw_conv(90 + servoTest));
     delay(30);
   }
   for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
-    servo1.write(90 + servoTest);
+    yaw.write(yaw_conv(90 + servoTest));
     delay(30);
   }
 
@@ -62,11 +64,11 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
 
   // Servo 2 Test
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
-    servo2.write(90 + servoTest);
+    pitch.write(pitch_conv(90 + servoTest));
     delay(30);
   }
   for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
-    servo2.write(90 + servoTest);
+    pitch.write(pitch_conv(90 + servoTest));
     delay(30);
   }
 
@@ -74,20 +76,20 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
 
   // Servo 1 & 2 Test
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
-    servo1.write(90 + servoTest);
-    servo2.write(90 + servoTest);
+    yaw.write(yaw_conv(90 + servoTest));
+    pitch.write(pitch_conv(90 + servoTest));
     delay(30);
   }
   for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
-    servo1.write(90 + servoTest);
-    servo2.write(90 + servoTest);
+    yaw.write(yaw_conv(90 + servoTest));
+    pitch.write(pitch_conv(90 + servoTest));
     delay(30);
   }
 
   delay(30);
   // BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
-  servo1.write(90);
-  servo2.write(90);
+  yaw.write(yaw_conv(90));
+  pitch.write(pitch_conv(90));
 }
 
 void initLoadCells(Vehicle &State) {
@@ -207,39 +209,32 @@ void thrustInfo(Vehicle &State) {
 
     getThrust(State);
 
+    // Constants based on vehicle
     double r = 3.0;
     double R = 5.0;
+
     // Vector math to aqcuire thrust vector components
-    // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
-    // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
     State.thrust = State.lc0_processed + State.lc1_processed +
                    State.lc2_processed + State.lc3_processed;
     State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
     State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
     State.Fz =
         sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
-        (state.mass * g);
+        (State.mass * g);
 
   } else {
     State.thrust = 0.0;
   }
 }
 
-void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
+void processTVC(Vehicle &State, PWMServo &yaw, PWMServo &pitch) {
   State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
   State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
   State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
              (State.mass * g);
 
-  // Calculate moment created by Fx and Fy
-  State.momentX = State.Fx * State.momentArm;
-  State.momentY = State.Fy * State.momentArm;
-  State.momentZ = 0.0;
-
-  State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
-
-  servo1.write(90 + State.xServoDegs);
-  servo2.write(90 + State.yServoDegs);
+  yaw.write(yaw_conv(State.xServoDegs));
+  pitch.write(pitch_conv(State.yServoDegs));
 }
 
 void write2CSV(Vehicle &State) {
@@ -395,4 +390,21 @@ double loadCellFilter(double current, double previous) {
   } else {
     return current;
   }
+}
+
+float yaw_conv(float thetad) {
+  float h = .2875;
+  float H = 1.6;
+
+  float theta = thetad * M_PI / 180.0;
+
+  return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
+}
+float pitch_conv(float thetad) {
+  float h = .2875;
+  float H = 1.2;
+
+  float theta = thetad * M_PI / 180.0;
+
+  return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
 }
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 8e1a0f4..0dde800 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -26,12 +26,8 @@ unsigned long last, initTime;
 #include <HX711.h>
 #include <PWMServo.h>
 
-PWMServo servo1;
-PWMServo servo2;
-
-const int pin_servo1 = 33;
-const int pin_servo2 = 29;
-const int pin_igniter = 7;
+PWMServo yaw;
+PWMServo pitch;
 
 int servoPos = 90; // variable to store the servo position;
 
@@ -48,9 +44,9 @@ void setup() {
   initLoadCells(State);
   init_Vehicle(State);
 
-  servo1.attach(pin_servo1);
-  servo2.attach(pin_servo2);
-  testGimbal(servo1, servo2);
+  yaw.attach(29);
+  pitch.attach(33);
+  testGimbal(yaw, pitch);
   Serial.println("Servos Tested");
 
   delay(3000);
@@ -75,7 +71,7 @@ void loop() {
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
-  processTVC(State, servo1, servo2);
+  processTVC(State, yaw, pitch);
 
   write2CSV(State);