Now printing entire sim to simOut.csv on SD card

include/native.h

@@ -117,6 +117,10 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
 }
 
 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;
+
   double landing_angle =
       pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
 

include/sim.h

@@ -235,9 +235,6 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
   stateVector.PIDy[t] = State.PIDy;
 
   stateVector.thrust[t] = State.thrust;
-
-  // Set "prev" values for next timestep
-  PrevState = State;
 }
 
 double derivative(double current, double previous, double step) {

include/teensy.h

@@ -4,12 +4,16 @@
 #include <SD.h>
 #include <SPI.h>
 
+double loadCellCalibrate();
+void initFile();
 void thrustInfo(struct Vehicle &);
 void processTVC(struct Vehicle &);
-void write2CSV(struct outVector &, struct Vehicle &);
+void write2CSV(struct Vehicle &);
 void printSimResults(struct Vehicle &);
+void teensyAbort();
 
-double loadCellCalibrate();
+const int chipSelect = BUILTIN_SDCARD;
+File dataFile;
 
 double loadCellCalibrate() {
   // place code to calibrate load cells in here
@@ -21,6 +25,38 @@ double loadCellCalibrate() {
   return loadTotal / 10;
 }
 
+void initFile() {
+  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. \n\nABORTING SIMULATION");
+    teensyAbort();
+  }
+  Serial.println("Card initialized.");
+
+  // Delete any previous existing files
+  if (SD.exists("simOut.csv")) {
+    SD.remove("simOut.csv");
+  }
+
+  // Open simOut.csv
+  dataFile = SD.open("simOut.csv", FILE_WRITE);
+  if (dataFile) {
+    Serial.println("File successfully opened!");
+
+  } else {
+    // if the file didn't open, print an error:
+    Serial.println("Error opening simOut.csv. \n\nABORTING SIMULATION");
+    teensyAbort();
+  }
+
+  // File Header
+  dataFile.println(
+      "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
+      "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust");
+}
+
 void thrustInfo(Vehicle &State) {
   if (State.time == 0) {
     Serial.println("WARNING: thrustInfo not implemented for TEENSY");
@@ -80,42 +116,67 @@ void processTVC(Vehicle &State) {
   State.momentZ = 0;
 }
 
-void write2CSV(outVector &stateVector, Vehicle &State) {
-  // Serial.println("WARNING: write2CSV not implemented for TEENSY");
+void write2CSV(Vehicle &State) {
+  dataFile.print(State.time);
+  dataFile.print(",");
 
-  const int chipSelect = BUILTIN_SDCARD;
+  dataFile.print(State.x);
+  dataFile.print(",");
+  dataFile.print(State.y);
+  dataFile.print(",");
+  dataFile.print(State.z);
+  dataFile.print(",");
 
-  Serial.print("Initializing SD card...");
+  dataFile.print(State.vx);
+  dataFile.print(",");
+  dataFile.print(State.vy);
+  dataFile.print(",");
+  dataFile.print(State.vz);
+  dataFile.print(",");
 
-  // 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.");
+  dataFile.print(State.ax);
+  dataFile.print(",");
+  dataFile.print(State.ay);
+  dataFile.print(",");
+  dataFile.print(State.az);
+  dataFile.print(",");
 
-  // Delete the file so it can be created again at the begining of the loop
-  if (SD.exists("simOut.csv")) {
-    SD.remove("simOut.csv");
-  }
+  dataFile.print(State.yaw * 180 / M_PI);
+  dataFile.print(",");
+  dataFile.print(State.pitch * 180 / M_PI);
+  dataFile.print(",");
+  dataFile.print(State.roll * 180 / M_PI);
+  dataFile.print(",");
 
-  // Open simOut.csv
-  File dataFile = SD.open("simOut.csv", FILE_WRITE);
-  if (dataFile) {
-    Serial.println("File successfully opened!");
-    dataFile.println("It works!");
+  dataFile.print(State.yawdot * 180 / M_PI);
+  dataFile.print(",");
+  dataFile.print(State.pitchdot * 180 / M_PI);
+  dataFile.print(",");
+  dataFile.print(State.rolldot * 180 / M_PI);
+  dataFile.print(",");
 
-    // close the file:
-    dataFile.close();
+  dataFile.print(State.xServoDegs);
+  dataFile.print(",");
+  dataFile.print(State.yServoDegs);
+  dataFile.print(",");
 
-  } else {
-    // if the file didn't open, print an error:
-    Serial.println("Error opening simOut.csv");
-  }
+  dataFile.print(State.thrustFiring);
+  dataFile.print(",");
+
+  dataFile.print(State.PIDx);
+  dataFile.print(",");
+  dataFile.print(State.PIDy);
+  dataFile.print(",");
+
+  dataFile.print(State.thrust);
+  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;
+  
   double landing_angle =
       pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
 
@@ -138,5 +199,16 @@ void printSimResults(Vehicle &State) {
   Serial.print("Final Velocity: [" + String(State.vx) + ", " +
                String(State.vy) + ", " + String(State.vz) + "]\n");
 
-  Serial.print("\n\nSimulation Complete\n");
+  Serial.print("\nSimulation Complete\n");
+}
+
+void closeFile() {
+  // close the file:
+  dataFile.close();
+  Serial.println("File closed\n");
+}
+
+void teensyAbort() {
+  while (1) {
+  }
 }

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