diff --git a/include/sim.h b/include/sim.h
index 369a030..6fd1b11 100644
--- a/include/sim.h
+++ b/include/sim.h
@@ -9,7 +9,7 @@ void pidController(struct Vehicle &, struct Vehicle &);
 void TVC(struct Vehicle &, struct Vehicle &);
 void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
 void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
-void write2CSV(struct outVector &, struct Vehicle &);
+void write2CSV(struct outVector &, struct Vehicle &, int t);
 double derivative(double current, double previous, double step);
 double integral(double currentChange, double prevValue, double dt);
 double limit(double value, double upr, double lwr);
@@ -35,11 +35,13 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
     pidController(State, PrevState);
     TVC(State, PrevState);
     state2vec(State, PrevState, stateVector, t);
+    //std::cout << State.vz << "\n";
 
     t += State.stepSize;
-  } while ((State.z > 0.0) || (State.thrust > 0.1));
-
-  write2CSV(stateVector, State);
+  } while ((State.z > 0.0));
+std::cout << t << "\n";
+  write2CSV(stateVector, State, t);
+  std::cout << t << "\n";
 
   bool pass = 1;
 
@@ -91,9 +93,8 @@ void burnStartTimeCalc(Vehicle &State) {
       thrust = -195.78 * i + 675.11;
 
     velocity = (((thrust / mass) + g) * dt) + velocity;
-    h = velocity * dt + h;
+    h = (((thrust / mass) + g) * dt) + h;
   }
-
   State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
   State.burnVelocity = velocity;               // terminal velocity
 
@@ -340,7 +341,7 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
   PrevState = State;
 }
 
-void write2CSV(outVector &stateVector, Vehicle &State) {
+void write2CSV(outVector &stateVector, Vehicle &State, int t) {
 
   // Deleting any previous output file
   if (remove("simOut.csv") != 0)
@@ -359,38 +360,38 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
           << std::endl;
 
   // writing to output file
-  for (int t = 0; t < State.simTime; t += State.stepSize) {
-    outfile << t << ", ";
+  for (int i = 0; i < t; i += State.stepSize) {
+    outfile << i << ", ";
 
-    outfile << stateVector.x[t] << ",";
-    outfile << stateVector.y[t] << ",";
-    outfile << stateVector.z[t] << ",";
+    outfile << stateVector.x[i] << ",";
+    outfile << stateVector.y[i] << ",";
+    outfile << stateVector.z[i] << ",";
 
-    outfile << stateVector.vx[t] << ",";
-    outfile << stateVector.vy[t] << ",";
-    outfile << stateVector.vz[t] << ",";
+    outfile << stateVector.vx[i] << ",";
+    outfile << stateVector.vy[i] << ",";
+    outfile << stateVector.vz[i] << ",";
 
-    outfile << stateVector.ax[t] << ",";
-    outfile << stateVector.ay[t] << ",";
-    outfile << stateVector.az[t] << ",";
+    outfile << stateVector.ax[i] << ",";
+    outfile << stateVector.ay[i] << ",";
+    outfile << stateVector.az[i] << ",";
 
-    outfile << stateVector.yaw[t] * 180 / M_PI << ",";
-    outfile << stateVector.pitch[t] * 180 / M_PI << ",";
-    outfile << stateVector.roll[t] * 180 / M_PI << ",";
+    outfile << stateVector.yaw[i] * 180 / M_PI << ",";
+    outfile << stateVector.pitch[i] * 180 / M_PI << ",";
+    outfile << stateVector.roll[i] * 180 / M_PI << ",";
 
-    outfile << stateVector.yawdot[t] * 180 / M_PI << ",";
-    outfile << stateVector.pitchdot[t] * 180 / M_PI << ",";
-    outfile << stateVector.rolldot[t] * 180 / M_PI << ",";
+    outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
+    outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
+    outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
 
-    outfile << stateVector.servo1[t] << ",";
-    outfile << stateVector.servo2[t] << ",";
+    outfile << stateVector.servo1[i] << ",";
+    outfile << stateVector.servo2[i] << ",";
 
-    outfile << stateVector.thrustFiring[t] << ",";
+    outfile << stateVector.thrustFiring[i] << ",";
 
-    outfile << stateVector.PIDx[t] << ",";
-    outfile << stateVector.PIDy[t] << ",";
+    outfile << stateVector.PIDx[i] << ",";
+    outfile << stateVector.PIDy[i] << ",";
 
-    outfile << stateVector.thrust[t] << std::endl;
+    outfile << stateVector.thrust[i] << std::endl;
   }
 
   outfile.close();
diff --git a/src/main.cpp b/src/main.cpp
index f3bf0d1..ee7d759 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -28,9 +28,8 @@ int main() {
   State.vz = 0; // [m/s]
 
   // Initial YPR
-
-  State.yaw = 75 * M_PI / 180;   // [rad]
-  State.pitch = 30 * M_PI / 180; // [rad]
+  State.yaw = 10 * M_PI / 180;   // [rad]
+  State.pitch = 5 * M_PI / 180; // [rad]
   State.roll = 0 * M_PI / 180;   // [rad]
 
   // Initial YPRdot