diff --git a/include/Vehicle.h b/include/Vehicle.h
index 31de448..921c953 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -20,7 +20,7 @@ struct Vehicle {
   double burntime;
   double burnVelocity;
   double thrust, burnElapsed, burnStart;
-  bool thrustFiring = false;
+  int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn
 
   double PIDx, PIDy, Fx, Fy, Fz;
   double momentX, momentY, momentZ;
@@ -28,47 +28,47 @@ struct Vehicle {
   double I11, I22, I33;
   double I11dot, I22dot, I33dot;
 
-  int maxServo;
-  int maxServoRate;
+  double maxServo;
+  double maxServoRate;
   double xServoDegs, yServoDegs;
   double xServoDegsDot, yServoDegsDot;
 
   double Kp, Ki, Kd;
   double yError, yPrevError;
   double pError, pPrevError;
-  double i_yError, i_pError = 0;
+  double i_yError, i_pError = 0.0;
   double d_yError, d_pError;
 
   double simTime;
-  int stepSize;
+  double stepSize;
 
-  int time = 0;
+  double time = 0.0;
 };
 
 void init_Vehicle(Vehicle &State) {
   // PID Gains
   State.Kp = -6.8699;
-  State.Ki = 0;
+  State.Ki = 0.0;
   State.Kd = -0.775;
 
   // Initial Velocity
-  State.vx = 0; // [m/s]
-  State.vy = 0; // [m/s]
-  State.vz = 0; // [m/s]
+  State.vx = 0.0; // [m/s]
+  State.vy = 0.0; // [m/s]
+  State.vz = 0.0; // [m/s]
 
   // Initial YPR
-  State.yaw = 45 * M_PI / 180;   // [rad]
-  State.pitch = 45 * M_PI / 180; // [rad]
-  State.roll = 0 * M_PI / 180;   // [rad]
+  State.yaw = 45.0 * M_PI / 180.0;   // [rad]
+  State.pitch = 45.0 * M_PI / 180.0; // [rad]
+  State.roll = 0.0 * M_PI / 180.0;   // [rad]
 
   // Initial YPRdot
-  State.yawdot = 1 * M_PI / 180;    // [rad/s]
-  State.pitchdot = -1 * M_PI / 180; // [rad/s]
-  State.rolldot = 0 * M_PI / 180;   // [rad/s]
+  State.yawdot = 1.0 * M_PI / 180.0;    // [rad/s]
+  State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s]
+  State.rolldot = 0.0 * M_PI / 180.0;   // [rad/s]
 
   // Servo Limitation
-  State.maxServo = 7;       // [degs]
-  State.maxServoRate = 360; // [degs/sec]
+  State.maxServo = 7.0;       // [degs]
+  State.maxServoRate = 360.0; // [degs/sec]
 
   // Vehicle Properties
   State.massInitial = 1.2;       // [kg]
@@ -77,7 +77,7 @@ void init_Vehicle(Vehicle &State) {
   State.momentArm = 0.145;       // [m]
 
   // Sim Step Size
-  State.stepSize = 1; // [ms]
+  State.stepSize = 1.0; // [ms]
 
   // Other Properties
   State.massPropellant = 0.06;                                  // [kg]
diff --git a/include/native.h b/include/native.h
index 9031117..d12f87a 100644
--- a/include/native.h
+++ b/include/native.h
@@ -8,41 +8,23 @@ void write2CSV(struct outVector &, struct Vehicle &);
 void printSimResults(struct Vehicle &);
 
 void thrustInfo(Vehicle &State) {
-
-  if (State.burnElapsed != 2000) {
-    // determine where in the thrust curve we're at based on elapsed burn time
-    // as well as current mass
-    State.burnElapsed = (State.time - State.burnStart) / 1000;
-    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
-  }
-
-  else if (abs(State.burnVelocity + State.vz) < 0.001) {
+  if ((std::abs(State.burnVelocity + State.vz) < 1.03) &&
+      (State.thrustFiring == 0)) {
     // Start burn
     State.burnStart = State.time;
-    State.burnElapsed = 0;
-  }
+    State.burnElapsed = 0.0;
+    State.thrustFiring = 1;
 
-  else
-    State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
+    getThrust(State);
 
-  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
-    State.thrustFiring = true;
-    State.thrust = 65.165 * State.burnElapsed - 2.3921;
+  } else if (State.thrustFiring == 1) {
+    State.burnElapsed = (State.time - State.burnStart) / 1000.0;
+    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
 
-  } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
-    State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
-                   11.609 * pow(State.burnElapsed, 5) +
-                   60.739 * pow(State.burnElapsed, 4) -
-                   162.99 * pow(State.burnElapsed, 3) +
-                   235.6 * pow(State.burnElapsed, 2) -
-                   174.43 * State.burnElapsed + 67.17;
+    getThrust(State);
 
-  else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
-    State.thrust = -195.78 * State.burnElapsed - 675.11;
-
-  if (State.burnElapsed > 3.45) {
-    State.thrustFiring = false;
-    State.thrust = 0;
+  } else {
+    State.thrust = 0.0;
   }
 }
 
diff --git a/include/sim.h b/include/sim.h
index 0045eff..d107292 100644
--- a/include/sim.h
+++ b/include/sim.h
@@ -11,6 +11,7 @@ void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &);
 double derivative(double current, double previous, double step);
 double integral(double currentChange, double prevValue, double dt);
 double limit(double value, double upr, double lwr);
+void getThrust(struct Vehicle &);
 
 // Any parameters that are constants should be declared here instead of
 // buried in code
@@ -19,7 +20,7 @@ double const g = -9.81;
 
 void burnStartTimeCalc(Vehicle &State) {
   double velocity = State.vz;
-  double h = 0;
+  double h = 0.0;
 
   double mass, thrust;
 
@@ -37,39 +38,39 @@ void burnStartTimeCalc(Vehicle &State) {
     else if ((i > 3.382) && (i < 3.46))
       thrust = -195.78 * i + 675.11;
     else
-      thrust = 0;
+      thrust = 0.0;
 
     velocity = (((thrust / mass) + g) * dt) + velocity;
     h = (((thrust / mass) + g) * dt) + h;
   }
-  State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
-  State.z = 18.9;
+  State.z = h + (pow(velocity, 2) / (2.0 * -g)); // starting height
+  State.z = 19.05;
   State.burnVelocity = velocity; // terminal velocity
 
   double burnStartTime = State.burnVelocity / -g;
-  State.simTime = (State.burntime + burnStartTime) * 1000;
+  State.simTime = (State.burntime + burnStartTime) * 1000.0;
 }
 
 void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
 
   // Moment of Inertia
-  State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
-                            pow(State.vehicleRadius, 2) / 4);
-  State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
-                            pow(State.vehicleRadius, 2) / 4);
+  State.I11 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
+                            pow(State.vehicleRadius, 2) / 4.0);
+  State.I22 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
+                            pow(State.vehicleRadius, 2) / 4.0);
   State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
 
   // Idot
   if (State.time < 0.1) {
-    State.I11dot = 0;
-    State.I22dot = 0;
-    State.I33dot = 0;
+    State.I11dot = 0.0;
+    State.I22dot = 0.0;
+    State.I33dot = 0.0;
 
-    State.x = 0;
-    State.y = 0;
+    State.x = 0.0;
+    State.y = 0.0;
 
-    State.ax = 0;
-    State.ay = 0;
+    State.ax = 0.0;
+    State.ay = 0.0;
     State.az = State.Fz / State.massInitial;
 
   } else {
@@ -145,7 +146,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
     State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
 
     // TVC block properly
-
     State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
                   State.Kd * State.d_yError) /
                  State.momentArm;
@@ -154,8 +154,8 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
                  State.momentArm;
 
   } else {
-    State.PIDx = 0;
-    State.PIDy = 0;
+    State.PIDx = 0.0;
+    State.PIDy = 0.0;
   }
 
   // PID Force Limiter
@@ -166,18 +166,18 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
 void TVC(Vehicle &State, Vehicle &PrevState) {
   if (State.thrust < 0.1) {
     // Define forces and moments for t = 0
-    State.Fx = 0;
-    State.Fy = 0;
+    State.Fx = 0.0;
+    State.Fy = 0.0;
     State.Fz = g * State.massInitial;
 
-    State.momentX = 0;
-    State.momentY = 0;
-    State.momentZ = 0;
+    State.momentX = 0.0;
+    State.momentY = 0.0;
+    State.momentZ = 0.0;
 
   } else {
     // Convert servo position to degrees for comparison to max allowable
-    State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
-    State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
+    State.xServoDegs = (180.0 / M_PI) * asin(State.PIDx / State.thrust);
+    State.yServoDegs = (180.0 / M_PI) * asin(State.PIDy / State.thrust);
 
     // Limit Servo Position
     State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
@@ -238,12 +238,12 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
 }
 
 double derivative(double current, double previous, double step) {
-  double dxdt = (current - previous) / (step / 1000);
+  double dxdt = (current - previous) / (step / 1000.0);
   return dxdt;
 }
 
 double integral(double currentChange, double prevValue, double dt) {
-  return (currentChange * dt / 1000) + prevValue;
+  return (currentChange * dt / 1000.0) + prevValue;
 }
 
 double limit(double value, double upr, double lwr) {
@@ -255,4 +255,26 @@ double limit(double value, double upr, double lwr) {
     value = value;
 
   return value;
+}
+
+void getThrust(Vehicle &State) {
+  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
+    State.thrust = 65.165 * State.burnElapsed - 2.3921;
+
+  } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
+    State.thrust = 0.8932 * pow(State.burnElapsed, 6.0) -
+                   11.609 * pow(State.burnElapsed, 5.0) +
+                   60.739 * pow(State.burnElapsed, 4.0) -
+                   162.99 * pow(State.burnElapsed, 3.0) +
+                   235.6 * pow(State.burnElapsed, 2.0) -
+                   174.43 * State.burnElapsed + 67.17;
+
+  else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46)) {
+    State.thrust = -195.78 * State.burnElapsed + 675.11;
+  }
+
+  if (State.burnElapsed > 3.45) {
+    State.thrustFiring = 2;
+    State.thrust = 0.0;
+  }
 }
\ No newline at end of file
diff --git a/include/teensy.h b/include/teensy.h
index 027b542..62d7076 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -17,11 +17,11 @@ File dataFile;
 double loadCellCalibrate() {
   // place code to calibrate load cells in here
   double loadTotal;
-  for (double t = 0; t == 10; ++t) {
-    loadTotal += 1;
+  for (double t = 0.0; t == 10.0; ++t) {
+    loadTotal += 1.0;
     delay(15);
   }
-  return loadTotal / 10;
+  return loadTotal / 10.0;
 }
 
 void initFile() {
@@ -35,11 +35,10 @@ void initFile() {
   Serial.println("Card initialized.");
 
   int i = 1;
-  Serial.print("simOut_" + String(i) + ".csv");
   const char *fileName;
 
   if (SD.exists("simOut.csv")) {
-    while (i > 0) {
+    while (i > 0.0) {
       fileName = ("simOut_" + String(i) + ".csv").c_str();
 
       if (!SD.exists(fileName)) {
@@ -54,7 +53,7 @@ void initFile() {
           Serial.println("Error opening output file. \n\nABORTING SIMULATION");
           teensyAbort();
         }
-        i = 0;
+        i = 0.0;
 
       } else {
         i++;
@@ -84,120 +83,103 @@ void thrustInfo(Vehicle &State) {
     Serial.println("WARNING: thrustInfo not implemented for TEENSY");
   }
 
-  if (State.burnElapsed != 2000) {
-    // determine where in the thrust curve we're at based on elapsed burn time
-    // as well as current mass
-    State.burnElapsed = (State.time - State.burnStart) / 1000;
-    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
-  }
-
-  else if (abs(State.burnVelocity + State.vz) < 0.001) {
+  if ((abs(State.burnVelocity + State.vz) < 1.03) &&
+      (State.thrustFiring == 0)) {
     // Start burn
     State.burnStart = State.time;
-    State.burnElapsed = 0;
-  }
+    State.burnElapsed = 0.0;
+    State.thrustFiring = 1;
 
-  else
-    State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
+    getThrust(State);
 
-  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
-    State.thrustFiring = true;
-    State.thrust = 65.165 * State.burnElapsed - 2.3921;
+  } else if (State.thrustFiring == 1) {
+    State.burnElapsed = (State.time - State.burnStart) / 1000.0;
+    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
 
-  } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
-    State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
-                   11.609 * pow(State.burnElapsed, 5) +
-                   60.739 * pow(State.burnElapsed, 4) -
-                   162.99 * pow(State.burnElapsed, 3) +
-                   235.6 * pow(State.burnElapsed, 2) -
-                   174.43 * State.burnElapsed + 67.17;
+    getThrust(State);
 
-  else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
-    State.thrust = -195.78 * State.burnElapsed - 675.11;
-
-  if (State.burnElapsed > 3.45) {
-    State.thrustFiring = false;
-    State.thrust = 0;
+  } else {
+    State.thrust = 0.0;
   }
 }
 
 void processTVC(Vehicle &State) {
-  if (State.time == 0) {
+  if (State.time == 0.0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
 
   // Vector math to aqcuire thrust vector components
-  State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
-  State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
+  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;
+  State.momentZ = 0.0;
 }
 
 void write2CSV(Vehicle &State) {
-  dataFile.print(State.time);
+  dataFile.print(String(State.time, 5));
   dataFile.print(",");
 
-  dataFile.print(State.x);
+  dataFile.print(String(State.x, 5));
   dataFile.print(",");
-  dataFile.print(State.y);
+  dataFile.print(String(State.y, 5));
   dataFile.print(",");
-  dataFile.print(State.z);
+  dataFile.print(String(State.z, 5));
   dataFile.print(",");
 
-  dataFile.print(State.vx);
+  dataFile.print(String(State.vx, 5));
   dataFile.print(",");
-  dataFile.print(State.vy);
+  dataFile.print(String(State.vy, 5));
   dataFile.print(",");
-  dataFile.print(State.vz);
+  dataFile.print(String(State.vz, 5));
   dataFile.print(",");
 
-  dataFile.print(State.ax);
+  dataFile.print(String(State.ax, 5));
   dataFile.print(",");
-  dataFile.print(State.ay);
+  dataFile.print(String(State.ay, 5));
   dataFile.print(",");
-  dataFile.print(State.az);
+  dataFile.print(String(State.az, 5));
   dataFile.print(",");
 
-  dataFile.print(State.yaw * 180 / M_PI);
+  dataFile.print(String(State.yaw * 180.0 / M_PI, 5));
   dataFile.print(",");
-  dataFile.print(State.pitch * 180 / M_PI);
+  dataFile.print(String(State.pitch * 180.0 / M_PI, 5));
   dataFile.print(",");
-  dataFile.print(State.roll * 180 / M_PI);
+  dataFile.print(String(State.roll * 180.0 / M_PI, 5));
   dataFile.print(",");
 
-  dataFile.print(State.yawdot * 180 / M_PI);
+  dataFile.print(String(State.yawdot * 180.0 / M_PI, 5));
   dataFile.print(",");
-  dataFile.print(State.pitchdot * 180 / M_PI);
+  dataFile.print(String(State.pitchdot * 180.0 / M_PI, 5));
   dataFile.print(",");
-  dataFile.print(State.rolldot * 180 / M_PI);
+  dataFile.print(String(State.rolldot * 180.0 / M_PI, 5));
   dataFile.print(",");
 
-  dataFile.print(State.xServoDegs);
+  dataFile.print(String(State.xServoDegs, 5));
   dataFile.print(",");
-  dataFile.print(State.yServoDegs);
+  dataFile.print(String(State.yServoDegs, 5));
   dataFile.print(",");
 
-  dataFile.print(State.thrustFiring);
+  dataFile.print(String(State.thrustFiring, 5));
   dataFile.print(",");
 
-  dataFile.print(State.PIDx);
+  dataFile.print(String(State.PIDx, 5));
   dataFile.print(",");
-  dataFile.print(State.PIDy);
+  dataFile.print(String(State.PIDy, 5));
   dataFile.print(",");
 
-  dataFile.print(State.thrust);
+  dataFile.print(String(State.thrust, 5));
   dataFile.print("\n");
 }
 
 void printSimResults(Vehicle &State) {
-  State.yaw = State.yaw * 180 / M_PI;
-  State.pitch = State.pitch * 180 / M_PI;
-  State.roll = State.roll * 180 / M_PI;
+  State.yaw = State.yaw * 180.0 / M_PI;
+  State.pitch = State.pitch * 180.0 / M_PI;
+  State.roll = State.roll * 180.0 / M_PI;
 
   double landing_angle =
       pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
diff --git a/matlabHelpers/simPlot.m b/matlabHelpers/simPlot.m
index 45e56aa..ba93e0f 100644
--- a/matlabHelpers/simPlot.m
+++ b/matlabHelpers/simPlot.m
@@ -30,6 +30,7 @@ Servo2      = T(:, 18);
 PIDx      = T(:, 20);
 PIDy      = T(:, 21);
 
+thrust = T(:, 22);
 % Acceleration
 subplot(3, 1, 1)
 plot(t, az)
@@ -91,3 +92,12 @@ plot(t, Servo2)
 title('Servo 2 Position vs Time')
 xlabel('Time (ms)')
 ylabel('Servo 2 Position (rad)')
+
+figure(4)
+
+% Servo 1 Position
+
+plot(t, thrust)
+title('Thrust vs Time')
+xlabel('Time (ms)')
+ylabel('Thrust (N)')
diff --git a/src/main.cpp b/src/main.cpp
index 71d76d0..21d20d5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -37,9 +37,7 @@ void setup() {
 }
 #elif defined(TEENSY)
 void setup() {
-  pinMode(BUILTIN_LED, OUTPUT);
-  digitalWrite(BUILTIN_LED, HIGH);
-  delay(1000);
+  delay(5000);
   init_Vehicle(State);
   Serial.println("Simulated Vehicle Initalized");
   delay(1000);
@@ -67,10 +65,9 @@ void loop() {
 
   // Set "prev" values for next timestep
   PrevState = State;
-
   State.time += State.stepSize;
 
-  if (State.z < 0.0) {
+  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     write2CSV(stateVector, State);
     printSimResults(State);
     init_Vehicle(State);
@@ -92,11 +89,12 @@ void loop() {
 
   State.time += State.stepSize;
 
-  if (State.z < 0.0) {
+  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     printSimResults(State);
     Serial.println("Run duration:" + String(millis() - last) + " ms");
 
     closeFile();
+    delay(20000);
 
     Serial.println("SUCCESS");
     Serial.println("Aborting Sim");
@@ -112,7 +110,7 @@ int main() {
 
   do {
     loop();
-  } while ((State.z > 0.0));
+  } while ((State.z > 0.0) || (State.thrustFiring != 2));
 
   return 0;
 }