From cce72b9114a2475bf5afb74ecca40fb233d22c21 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Thu, 4 Nov 2021 15:56:55 -0700
Subject: [PATCH] Initial test function, still needs to be run on teensy to
 verify

---
 include/teensy.h | 34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/include/teensy.h b/include/teensy.h
index 49cf3ef..32e966f 100644
--- a/include/teensy.h
+++ b/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) {