Merge branch '15-integrate-with-sd-card' into 'main'

Resolve "Integrate with SD Card"

Closes #15

See merge request lander-team/lander-cpp!20

.vscode/extensions.json

@@ -1,8 +1,10 @@
 {
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
     "recommendations": [
         "ms-vscode.cpptools",
-        "wayou.vscode-todo-highlight",
+        "platformio.platformio-ide",
         "usernamehw.errorlens",
-        "platformio.platformio-ide"
+        "wayou.vscode-todo-highlight"
     ]
-}
+}

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/outVector.h

@@ -4,11 +4,7 @@
 #define OUTVECTOR_H
 
 struct outVector {
-#if defined(NATIVE) || defined(_WIN32)
-  int length = 100000; // current sim runs ~5000 steps, x2 just in case
-#elif defined(TEENSY)
-  int length = 1000; // current sim runs ~5000 steps, x2 just in case
-#endif
+  int length = 10000; // current sim runs ~5000 steps, x2 just in case
 
   std::vector<double> x = std::vector<double>(length, 0.0);
   std::vector<double> y = std::vector<double>(length, 0.0);

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

@@ -1,13 +1,19 @@
 #include "Vehicle.h"
 
 #include <Arduino.h>
-
-void thrustInfo(struct Vehicle &);
-void processTVC(struct Vehicle &);
-void write2CSV(struct outVector &, struct Vehicle &);
-void printSimResults(struct Vehicle &);
+#include <SD.h>
+#include <SPI.h>
 
 double loadCellCalibrate();
+void initFile();
+void thrustInfo(struct Vehicle &);
+void processTVC(struct Vehicle &);
+void write2CSV(struct Vehicle &);
+void printSimResults(struct Vehicle &);
+void teensyAbort();
+
+const int chipSelect = BUILTIN_SDCARD;
+File dataFile;
 
 double loadCellCalibrate() {
   // place code to calibrate load cells in here
@@ -19,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");
@@ -78,12 +116,67 @@ void processTVC(Vehicle &State) {
   State.momentZ = 0;
 }
 
-void write2CSV(outVector &stateVector, Vehicle &State) {
+void write2CSV(Vehicle &State) {
+  dataFile.print(State.time);
+  dataFile.print(",");
 
-  Serial.println("WARNING: write2CSV not implemented for TEENSY");
+  dataFile.print(State.x);
+  dataFile.print(",");
+  dataFile.print(State.y);
+  dataFile.print(",");
+  dataFile.print(State.z);
+  dataFile.print(",");
+
+  dataFile.print(State.vx);
+  dataFile.print(",");
+  dataFile.print(State.vy);
+  dataFile.print(",");
+  dataFile.print(State.vz);
+  dataFile.print(",");
+
+  dataFile.print(State.ax);
+  dataFile.print(",");
+  dataFile.print(State.ay);
+  dataFile.print(",");
+  dataFile.print(State.az);
+  dataFile.print(",");
+
+  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(",");
+
+  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(",");
+
+  dataFile.print(State.xServoDegs);
+  dataFile.print(",");
+  dataFile.print(State.yServoDegs);
+  dataFile.print(",");
+
+  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);
 
@@ -106,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

@@ -19,13 +19,13 @@ unsigned long last;
 
 #if defined(NATIVE) || defined(_WIN32)
 #include "native.h"
+outVector stateVector;
 #elif defined(TEENSY)
 #include "teensy.h"
 #endif
 
 Vehicle State;
 Vehicle PrevState;
-outVector stateVector;
 
 #if defined(NATIVE) || defined(_WIN32)
 void setup() {
@@ -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,24 +75,32 @@ 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);
+    Serial.println("Last run duration:" + String(millis() - last) + " ms");
+
+    closeFile();
+    delay(10000);
+
     Serial.println("Restarting Sim");
+    Serial.println(
+        "===============================================================");
   }
 }
 #endif