Merge branch 'make-teensy-compile' into 'main'

Make teensy compile

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

include/outVector.h

@@ -4,7 +4,11 @@
 #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
 
   std::vector<double> x = std::vector<double>(length, 0.0);
   std::vector<double> y = std::vector<double>(length, 0.0);

include/teensy.h

@@ -18,14 +18,93 @@ double loadCellCalibrate() {
   }
   return loadTotal / 10;
 }
-void thrustInfo() {
-  // place code to retrieve load cell data and convert to a thrust value in here
+
+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() {
-  // place code to turn angles from TVC function into commands in here
+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() {
-  // place code to make the teensy write to an output  file in here
+void write2CSV(outVector &stateVector, Vehicle &State) {
+
+  Serial.println("WARNING: write2CSV not implemented for TEENSY");
+}
+
+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");
 }

src/main.cpp

@@ -1,5 +1,6 @@
 #define M_PI 3.14159265359
 
+#if defined(NATIVE) || defined(_WIN32)
 #include <cmath>
 #include <fstream>
 #include <iostream>
@@ -7,6 +8,11 @@
 #include <stdio.h>
 #include <string>
 #include <vector>
+#elif defined(TEENSY)
+#include <Arduino.h>
+
+unsigned long last;
+#endif
 
 #include "Vehicle.h"
 #include "sim.h"
@@ -30,16 +36,23 @@ void setup() {
 }
 #elif defined(TEENSY)
 void setup() {
+  delay(1000);
   init_Vehicle(State);
+  Serial.println("Simulated Vehicle Initalized");
+  delay(1000);
 
   // Determine when to burn
   burnStartTimeCalc(State);
+  Serial.println("Starting Height Calculated");
+  delay(1000);
   loadCellCalibrate();
+  Serial.println("Load Cells Calibrated");
+  delay(1000);
 }
 #endif
 
+#if defined(NATIVE) || defined(_WIN32)
 void loop() {
-
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
@@ -52,8 +65,32 @@ void loop() {
   if (State.z < 0.0) {
     write2CSV(stateVector, State);
     printSimResults(State);
+    init_Vehicle(State);
   }
 }
+#elif defined(TEENSY)
+void loop() {
+  last = millis();
+  vehicleDynamics(State, PrevState);
+  thrustInfo(State);
+  pidController(State, PrevState);
+  TVC(State, PrevState);
+  processTVC(State);
+  // state2vec(State, PrevState, stateVector);
+
+  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("Restarting Sim");
+  }
+}
+#endif
 
 #if defined(_WIN32) || defined(linux)
 int main() {