Now printing entire sim to simOut.csv on SD card

src/main.cpp

@@ -48,6 +48,8 @@ void setup() {
   loadCellCalibrate();
   Serial.println("Load Cells Calibrated");
   delay(1000);
+  initFile();
+  delay(1000);
 }
 #endif
 
@@ -60,6 +62,9 @@ void loop() {
   processTVC(State);
   state2vec(State, PrevState, stateVector);
 
+  // Set "prev" values for next timestep
+  PrevState = State;
+
   State.time += State.stepSize;
 
   if (State.z < 0.0) {
@@ -70,25 +75,30 @@ void loop() {
 }
 #elif defined(TEENSY)
 void loop() {
+
   last = millis();
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
   processTVC(State);
-  // state2vec(State, PrevState, stateVector);
+  write2CSV(State);
+
+  // Set "prev" values for next timestep
+  PrevState = State;
 
   State.time += State.stepSize;
 
   if (State.z < 0.0) {
-    write2CSV(stateVector, State);
     printSimResults(State);
     init_Vehicle(State);
     Serial.println("Last run duration:" + String(millis() - last + " ms"));
-
-    delay(1000);
+    
+    closeFile();
+    delay(10000);
 
     Serial.println("Restarting Sim");
+    Serial.println("===============================================================");
   }
 }
 #endif