Lander-Embedded/include/teensy.h

#include "Vehicle.h"

#include <Arduino.h>
#include <SD.h>
#include <SPI.h>

void thrustInfo(struct Vehicle &);
void processTVC(struct Vehicle &);
void write2CSV(struct outVector &, struct Vehicle &);
void printSimResults(struct Vehicle &);

double loadCellCalibrate();

double loadCellCalibrate() {
  // place code to calibrate load cells in here
  double loadTotal;
  for (double t = 0; t == 10; ++t) {
    loadTotal += 1;
    delay(15);
  }
  return loadTotal / 10;
}

void thrustInfo(Vehicle &State) {
  if (State.time == 0) {
    Serial.println("WARNING: thrustInfo not implemented for TEENSY");
  }

  if (State.burnElapsed != 2000) {
    // determine where in the thrust curve we're at based on elapsed burn time
    // as well as current mass
    State.burnElapsed = (State.time - State.burnStart) / 1000;
    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
  }

  else if (abs(State.burnVelocity + State.vz) < 0.001) {
    // Start burn
    State.burnStart = State.time;
    State.burnElapsed = 0;
  }

  else
    State.burnElapsed = 2000; // arbitrary number to ensure we don't burn

  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
    State.thrustFiring = true;
    State.thrust = 65.165 * State.burnElapsed - 2.3921;

  } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
    State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
                   11.609 * pow(State.burnElapsed, 5) +
                   60.739 * pow(State.burnElapsed, 4) -
                   162.99 * pow(State.burnElapsed, 3) +
                   235.6 * pow(State.burnElapsed, 2) -
                   174.43 * State.burnElapsed + 67.17;

  else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
    State.thrust = -195.78 * State.burnElapsed - 675.11;

  if (State.burnElapsed > 3.45) {
    State.thrustFiring = false;
    State.thrust = 0;
  }
}

void processTVC(Vehicle &State) {
  if (State.time == 0) {
    Serial.println("WARNING: processTVC not implemented for TEENSY");
  }

  // Vector math to aqcuire thrust vector components
  State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
  State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
  State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
             (State.mass * g);

  // Calculate moment created by Fx and Fy
  State.momentX = State.Fx * State.momentArm;
  State.momentY = State.Fy * State.momentArm;
  State.momentZ = 0;
}

void write2CSV(outVector &stateVector, Vehicle &State) {
  // 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", FILE_WRITE);
  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) {
  double landing_angle =
      pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);

  double landing_velocity =
      pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);

  if (landing_angle < 5.0) {
    Serial.print("Landing Angle     < 5.0 degrees  | PASS | ");
  } else {
    Serial.print("Landing Angle     < 5.0 degrees  | FAIL | ");
  }
  Serial.print("Final Angles:   [" + String(State.yaw) + ", " +
               String(State.pitch) + "]\n");

  if (landing_velocity < 0.5) {
    Serial.print("Landing Velocity  < 0.5 m/s      | PASS | ");
  } else {
    Serial.print("Landing Velocity  < 0.5 m/s      | FAIL | ");
  }
  Serial.print("Final Velocity: [" + String(State.vx) + ", " +
               String(State.vy) + ", " + String(State.vz) + "]\n");

  Serial.print("\n\nSimulation Complete\n");
}