diff --git a/include/teensy.h b/include/teensy.h
index 8e7d600..d3e2895 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -1,12 +1,13 @@
 #include "Vehicle.h"
 #include <Arduino.h>
+#include <PWMServo.h>
 #include <SD.h>
 #include <SPI.h>
 
 double loadCellCalibrate();
 void initFile();
 void thrustInfo(struct Vehicle &);
-void processTVC(struct Vehicle &);
+void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
 void write2CSV(struct Vehicle &);
 void printSimResults(struct Vehicle &);
 void teensyAbort();
@@ -93,7 +94,7 @@ void thrustInfo(Vehicle &State) {
   }
 }
 
-void processTVC(Vehicle &State) {
+void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
   if (State.time == 0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
@@ -117,6 +118,9 @@ void processTVC(Vehicle &State) {
   State.momentZ = 0.0;
 
   State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
+
+  servo1.write(State.xServoDegs);
+  servo2.write(State.yServoDegs);
 }
 
 void write2CSV(Vehicle &State) {
diff --git a/src/main.cpp b/src/main.cpp
index 20bb4ba..00105e0 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -25,6 +25,8 @@ unsigned long last, initTime;
 
 #include "teensy.h"
 #include <HX711.h>
+#include <PWMServo.h>
+
 const int lc_clock = 23;
 
 const int pin_lc0 = 14;
@@ -43,6 +45,14 @@ HX711 lc1;
 HX711 lc2;
 HX711 lc3;
 
+PWMServo servo1;
+PWMServo servo2;
+
+const int pin_servo1 = 33;
+const int pin_servo2 = 29;
+
+int servoPos = 0; // variable to store the servo position;
+
 void setup() {
   Serial.begin(9600);
 
@@ -59,6 +69,12 @@ void setup() {
   State.lc1 = lc1.get_value();
   State.lc2 = lc2.get_value();
   State.lc3 = lc3.get_value();
+
+  servo1.attach(pin_servo1);
+  servo2.attach(pin_servo2);
+  servo1.write(0);
+  servo2.write(0);
+
   Serial.println("Simulated Vehicle Initalized");
 
   // Determine when to burn
@@ -69,27 +85,21 @@ void setup() {
 
   initTime = micros();
 }
-void loop() {
 
+void loop() {
   last = micros();
-  Serial.println(State.time);
+
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
-
-  processTVC(State);
-  delay(2);
+  processTVC(State, servo1, servo2);
 
   State.stepDuration = micros() - last;
   write2CSV(State);
 
-  //       \/  Comments are fee you can just write previous bro
-  // Set "prev" values for next timestep
+  // Set previous values for next timestep
   PrevState = State;
-
-  // state2vec(State, PrevState, stateVector); <-- can we delete?
-
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {