Initial test function, still needs to be run on teensy to verify

include/teensy.h

@@ -1,6 +1,8 @@
 #include "Vehicle.h"
 
 #include <Arduino.h>
+#include <SD.h>
+#include <SPI.h>
 
 void thrustInfo(struct Vehicle &);
 void processTVC(struct Vehicle &);
@@ -79,8 +81,38 @@ void processTVC(Vehicle &State) {
 }
 
 void write2CSV(outVector &stateVector, Vehicle &State) {
+  // Serial.println("WARNING: write2CSV not implemented for TEENSY");
 
-  Serial.println("WARNING: write2CSV not implemented for TEENSY");
+  const int chipSelect = BUILTIN_SDCARD;
+
+  Serial.print("Initializing SD card...");
+
+  // see if the card is present and can be initialized:
+  if (!SD.begin(chipSelect)) {
+    Serial.println("Card failed, or not present");
+    // don't do anything more:
+    return;
+  }
+  Serial.println("card initialized.");
+
+  // Delete the file so it can be created again at the begining of the loop
+  if (SD.exists("simOut.csv")) {
+    SD.remove("simOut.csv");
+  }
+
+  // Open simOut.csv
+  File dataFile = SD.open("simOut.csv");
+  if (dataFile) {
+    Serial.println("File successfully opened!");
+    dataFile.println("It works!");
+
+    // close the file:
+    dataFile.close();
+
+  } else {
+    // if the file didn't open, print an error:
+    Serial.println("Error opening simOut.csv");
+  }
 }
 
 void printSimResults(Vehicle &State) {