Merge branch '36-move-init_loadcells-to-teensy-h' into 'main'

Resolve "Move init_loadCells() to teensy.h"

Closes #36

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

.gitignore

@@ -2,3 +2,9 @@
 *.csv
 output
 public
+
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

.gitlab-ci.yml

@@ -13,14 +13,14 @@ build:
   stage: build
   script:
     - mkdir public
-    - g++ ./src/main.cpp -I include -o ./public/sim.out
+    - g++ ./src/main.cpp -I include -o ./public/sim.out -D NATIVE -Wall
     - ./public/sim.out
     - mv simOut.csv public
   artifacts:
     paths:
       - public
 
-plots:
+pythonplots:
   # stage: build
   image: python:3.9-buster
   dependencies:
@@ -32,11 +32,27 @@ plots:
     paths:
       - public
 
+juliaplots:
+  image: julia:1.7
+  dependencies:
+    - build
+  script:
+    - apt-get update
+    - apt-get install -y unzip
+    - julia -e 'using Pkg; Pkg.activate("./juliaHelpers/"); Pkg.instantiate()'
+    - julia --project=./juliaHelpers ./juliaHelpers/simplot.jl
+  artifacts:
+    paths:
+      - public
+  only:
+    - main
+
 pages:
   stage: deploy
   dependencies:
     - build
-    - plots
+    - pythonplots
+    - juliaplots
   script:
     - ""
   artifacts:

.vscode/c_cpp_properties.json

@@ -1,22 +0,0 @@
-{
-    "configurations": [
-        {
-            "name": "Win32",
-            "includePath": [
-                "${workspaceFolder}/**",
-                "${workspaceFolder}/include"
-            ],
-            "defines": [
-                "_DEBUG",
-                "UNICODE",
-                "_UNICODE"
-            ],
-            "windowsSdkVersion": "10.0.18362.0",
-            "compilerPath": "C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.25.28610\\bin\\Hostx64\\x64\\cl.exe",
-            "cStandard": "c17",
-            "cppStandard": "c++17",
-            "intelliSenseMode": "windows-msvc-x64"
-        }
-    ],
-    "version": 4
-}

.vscode/extensions.json

@@ -1,7 +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",
-        "usernamehw.errorlens"
+        "platformio.platformio-ide",
+        "usernamehw.errorlens",
+        "wayou.vscode-todo-highlight"
     ]
 }

.vscode/launch.json

@@ -1,29 +0,0 @@
-{
-    // Use IntelliSense to learn about possible attributes.
-    // Hover to view descriptions of existing attributes.
-    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
-    "version": "0.2.0",
-    "configurations": [
-        {
-            "name": "g++ - Build and debug simulation",
-            "type": "cppdbg",
-            "request": "launch",
-            "program": "${workspaceFolder}/main.exe",
-            "args": [],
-            "stopAtEntry": false,
-            "cwd": "${fileDirname}",
-            "environment": [],
-            "externalConsole": false,
-            "MIMode": "gdb",
-            "miDebuggerPath": "gdb.exe",
-            "setupCommands": [
-                {
-                    "description": "Enable pretty-printing for gdb",
-                    "text": "-enable-pretty-printing",
-                    "ignoreFailures": true
-                }
-            ],
-            "preLaunchTask": "buildSim"
-        }
-    ]
-}

.vscode/settings.json

@@ -42,7 +42,11 @@
         "vector": "cpp",
         "cctype": "cpp",
         "sstream": "cpp",
-        "string": "cpp"
+        "string": "cpp",
+        "chrono": "cpp",
+        "ratio": "cpp",
+        "thread": "cpp",
+        "string_view": "cpp"
     },
     "C_Cpp.clang_format_fallbackStyle": "LLVM",
     "editor.formatOnSave": true,

.vscode/tasks.json

@@ -2,7 +2,7 @@
 	"version": "2.0.0",
 	"tasks": [
 		{
-			"type": "cppbuild",
+			"type": "process",
 			"label": "buildSim",
 			"command": "g++",
 			"args": [
@@ -11,7 +11,8 @@
 				"-o",
 				"${workspaceFolder}/main.exe",
 				"-I",
-				"${workspaceFolder}/include"
+				"${workspaceFolder}/include",
+				"-Wall"
 			],
 			"options": {
 				"cwd": "${fileDirname}"

README.md

@@ -29,8 +29,12 @@ Download the raw CSV: https://lander-team.gitlab.io/lander-cpp/simOut.csv
 
 Download Linux Binaries: https://lander-team.gitlab.io/lander-cpp/sim.out
 
-![Accel-Vel-Alt vs Time](https://lander-team.gitlab.io/lander-cpp/AVA.png)
+### Plots
 
-![Euler Angles vs Time](https://lander-team.gitlab.io/lander-cpp/Angles.png)
+Click on the images for interactive versions. 
 
-![Servo Position vs Time](https://lander-team.gitlab.io/lander-cpp/Servos.png)
+<a href="https://lander-team.gitlab.io/lander-cpp/plot1.html" rel="Accel-Vel-ALt">![Accel-Vel-ALt](https://lander-team.gitlab.io/lander-cpp/plot1.svg)</a>
+
+<a href="https://lander-team.gitlab.io/lander-cpp/plot2.html" rel="Euler Angles vs Time">![Euler Angles vs Time](https://lander-team.gitlab.io/lander-cpp/plot2.svg)</a>
+
+<a href="https://lander-team.gitlab.io/lander-cpp/plot3.html" rel="Servo Position vs Time">![Servo Position vs Time](https://lander-team.gitlab.io/lander-cpp/plot3.svg)</a>

include/Vehicle.h

@@ -20,25 +20,76 @@ struct Vehicle {
   double burntime;
   double burnVelocity;
   double thrust, burnElapsed, burnStart;
-  bool thrustFiring = false;
+  int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn
 
-  double PIDx, PIDy, Fx, Fy, Fz;
+  double PIDx, PIDy, Fx, Fy, Fz, F;
   double momentX, momentY, momentZ;
 
   double I11, I22, I33;
   double I11dot, I22dot, I33dot;
 
-  int maxServo;
+  double maxServo;
+  double maxServoRate;
   double xServoDegs, yServoDegs;
+  double xServoDegsDot, yServoDegsDot;
 
   double Kp, Ki, Kd;
   double yError, yPrevError;
   double pError, pPrevError;
-  double i_yError, i_pError = 0;
+  double i_yError, i_pError = 0.0;
   double d_yError, d_pError;
 
   double simTime;
-  int stepSize;
+  double stepSize;
+  double stepDuration;
+
+  double time = 0.0;
+
+  double lc0, lc1, lc2, lc3 = 0.0;
+  double lc0_processed, lc1_processed, lc2_processed, lc3_processed = 0.0;
 };
 
+void init_Vehicle(Vehicle &State) {
+  // PID Gains
+  State.Kp = -6.8699;
+  State.Ki = 0.0;
+  State.Kd = -0.775;
+
+  // Initial Velocity
+  State.vx = 0.0; // [m/s]
+  State.vy = 0.0; // [m/s]
+  State.vz = 0.0; // [m/s]
+
+  // Initial YPR
+  State.yaw = 45.0 * M_PI / 180.0;   // [rad]
+  State.pitch = 45.0 * M_PI / 180.0; // [rad]
+  State.roll = 0.0 * M_PI / 180.0;   // [rad]
+
+  // Initial YPRdot
+  State.yawdot = 1.0 * M_PI / 180.0;    // [rad/s]
+  State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s]
+  State.rolldot = 0.0 * M_PI / 180.0;   // [rad/s]
+
+  // Servo Limitation
+  State.maxServo = 7.0;       // [degs]
+  State.maxServoRate = 360.0; // [degs/sec]
+
+  // Vehicle Properties
+  State.massInitial = 1.2;       // [kg]
+  State.vehicleHeight = 0.5318;  // [m]
+  State.vehicleRadius = 0.05105; // [m]
+  State.momentArm = 0.145;       // [m]
+
+  // Sim Step Size
+  State.stepSize = 5.0; // [ms]
+
+  // Other Properties
+  State.massPropellant = 0.06;                                  // [kg]
+  State.massBurnout = State.massInitial - State.massPropellant; // [kg]
+  State.burntime = 3.45 - 0.148;                                // [s]
+  State.mdot = State.massPropellant / State.burntime;           // [kg/s]
+  State.mass = State.massInitial;                               // [kg]
+  State.burnElapsed = 2000;                                     // [s]
+}
+
 #endif

include/native.h

@@ -0,0 +1,129 @@
+#include "Vehicle.h"
+
+#include <iostream>
+
+void thrustInfo(struct Vehicle &);
+void processTVC(struct Vehicle &);
+void write2CSV(struct outVector &, struct Vehicle &);
+void printSimResults(struct Vehicle &);
+
+void thrustInfo(Vehicle &State) {
+  if ((std::abs(State.burnVelocity + State.vz) < 1.03) &&
+      (State.thrustFiring == 0)) {
+    // Start burn
+    State.burnStart = State.time;
+    State.burnElapsed = 0.0;
+    State.thrustFiring = 1;
+
+    getThrust(State);
+
+  } else if (State.thrustFiring == 1) {
+    State.burnElapsed = (State.time - State.burnStart) / 1000.0;
+    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
+
+    getThrust(State);
+
+  } else {
+    State.thrust = 0.0;
+  }
+}
+
+void processTVC(Vehicle &State) {
+  // 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) {
+
+  // Deleting any previous output file
+  if (remove("simOut.csv") != 0)
+    perror("No file deletion necessary");
+  else
+    puts("Previous output file successfully deleted");
+
+  // Define and open output file "simOut.csv"
+  std::fstream outfile;
+  outfile.open("simOut.csv", std::ios::app);
+
+  // Output file header. These are the variables that we output - useful for
+  // debugging
+  outfile << "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
+             "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust"
+          << std::endl;
+
+  // writing to output file
+  for (int i = 0; i < State.time; i += State.stepSize) {
+    outfile << i << ", ";
+
+    outfile << stateVector.x[i] << ",";
+    outfile << stateVector.y[i] << ",";
+    outfile << stateVector.z[i] << ",";
+
+    outfile << stateVector.vx[i] << ",";
+    outfile << stateVector.vy[i] << ",";
+    outfile << stateVector.vz[i] << ",";
+
+    outfile << stateVector.ax[i] << ",";
+    outfile << stateVector.ay[i] << ",";
+    outfile << stateVector.az[i] << ",";
+
+    outfile << stateVector.yaw[i] * 180 / M_PI << ",";
+    outfile << stateVector.pitch[i] * 180 / M_PI << ",";
+    outfile << stateVector.roll[i] * 180 / M_PI << ",";
+
+    outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
+    outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
+    outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
+
+    outfile << stateVector.servo1[i] << ",";
+    outfile << stateVector.servo2[i] << ",";
+
+    outfile << stateVector.thrustFiring[i] << ",";
+
+    outfile << stateVector.PIDx[i] << ",";
+    outfile << stateVector.PIDy[i] << ",";
+
+    outfile << stateVector.thrust[i] << std::endl;
+  }
+
+  outfile.close();
+  std::cout << "simOut.csv created successfully.\n" << std::endl;
+}
+
+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);
+
+  double landing_velocity =
+      pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);
+
+  if (landing_angle < 5.0) {
+    std::cout << "Landing Angle     < 5.0 degrees  | PASS | ";
+  } else {
+    std::cout << "Landing Angle     < 5.0 degrees  | FAIL | ";
+  }
+  std::cout << "Final Angles:   [" << State.yaw << ", " << State.pitch << "]"
+            << std::endl;
+
+  if (landing_velocity < 1.0) {
+    std::cout << "Landing Velocity  < 1.0 m/s      | PASS | ";
+  } else {
+    std::cout << "Landing Velocity  < 1.0 m/s      | FAIL | ";
+  }
+  std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
+            << State.vz << "]" << std::endl;
+
+  std::cout << std::endl << "Simulation Complete\n" << std::endl;
+}

include/outVector.h

@@ -4,7 +4,7 @@
 #define OUTVECTOR_H
 
 struct outVector {
-  int length = 100000; // current sim runs ~5000 steps, x2 just in case
+  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);
@@ -33,6 +33,8 @@ struct outVector {
 
   std::vector<double> PIDx = std::vector<double>(length, 0.0);
   std::vector<double> PIDy = std::vector<double>(length, 0.0);
+
+  std::vector<double> thrust = std::vector<double>(length, 0.0);
 };
 
 #endif

include/sim.h

@@ -4,74 +4,23 @@
 #include <iostream>
 
 void burnStartTimeCalc(struct Vehicle &);
-void thrustSelection(struct Vehicle &, int t);
 void pidController(struct Vehicle &, struct Vehicle &);
-void TVC(struct Vehicle &);
-void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
-void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
-void write2CSV(struct outVector &, struct Vehicle &);
-double derivative(double x2, double x1, double dt);
-double integral(double x2, double x1, double dt);
+void TVC(struct Vehicle &, struct Vehicle &);
+void vehicleDynamics(struct Vehicle &, struct Vehicle &);
+void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &);
+double derivative(double current, double previous, double step);
+double integral(double currentChange, double prevValue, double dt);
+double limit(double value, double upr, double lwr);
+void getThrust(struct Vehicle &);
 
-// Any parameters that are constants should be declared here instead of buried
-// in code
+// Any parameters that are constants should be declared here instead of
+// buried in code
 double const dt = 0.01;
 double const g = -9.81;
 
-bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
-
-  outVector stateVector;
-
-  // Determine when to burn
-  burnStartTimeCalc(State);
-
-  int t = 0;
-
-  // Start Sim
-  do {
-    vehicleDynamics(State, PrevState, t);
-    thrustSelection(State, t);
-    pidController(State, PrevState);
-    TVC(State);
-    state2vec(State, PrevState, stateVector, t);
-
-    t += State.stepSize;
-  } while ((State.z > 0.0) || (State.thrust > 0.1));
-
-  write2CSV(stateVector, State);
-
-  bool pass = 1;
-
-  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) {
-    std::cout << "   Landing Angle < 5°    | PASS | ";
-  } else {
-    std::cout << "   Landing Angle < 5°    | FAIL | ";
-    pass = pass * 0;
-  }
-  std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
-            << std::endl;
-
-  if (landing_velocity < 5.0) {
-    std::cout << "Landing Velocity < 5 m/s | PASS | ";
-  } else {
-    std::cout << "Landing Velocity < 5 m/s | FAIL | ";
-    pass = pass * 0;
-  }
-  std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
-            << State.vz << "]" << std::endl;
-
-  return pass;
-}
-
 void burnStartTimeCalc(Vehicle &State) {
   double velocity = State.vz;
-  double h = 0;
+  double h = 0.0;
 
   double mass, thrust;
 
@@ -88,37 +37,40 @@ void burnStartTimeCalc(Vehicle &State) {
 
     else if ((i > 3.382) && (i < 3.46))
       thrust = -195.78 * i + 675.11;
+    else
+      thrust = 0.0;
 
     velocity = (((thrust / mass) + g) * dt) + velocity;
-    h = velocity * dt + h;
+    h = (((thrust / mass) + g) * dt) + h;
   }
-
-  State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
+  State.z = h + (pow(velocity, 2) / (2.0 * -g)); // starting height
+  State.z = 19.05;
   State.burnVelocity = velocity; // terminal velocity
 
   double burnStartTime = State.burnVelocity / -g;
-  State.simTime = (State.burntime + burnStartTime) * 1000;
+  State.simTime = (State.burntime + burnStartTime) * 1000.0;
 }
 
-void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
+void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
+
   // Moment of Inertia
-  State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
-                            pow(State.vehicleRadius, 2) / 4);
-  State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
-                            pow(State.vehicleRadius, 2) / 4);
+  State.I11 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
+                            pow(State.vehicleRadius, 2) / 4.0);
+  State.I22 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
+                            pow(State.vehicleRadius, 2) / 4.0);
   State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
 
   // Idot
-  if (t < 0.1) {
-    State.I11dot = 0;
-    State.I22dot = 0;
-    State.I33dot = 0;
+  if (State.time < 0.1) {
+    State.I11dot = 0.0;
+    State.I22dot = 0.0;
+    State.I33dot = 0.0;
 
-    State.x = 0;
-    State.y = 0;
+    State.x = 0.0;
+    State.y = 0.0;
 
-    State.ax = 0;
-    State.ay = 0;
+    State.ax = 0.0;
+    State.ay = 0.0;
     State.az = State.Fz / State.massInitial;
 
   } else {
@@ -178,45 +130,6 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
   }
 }
 
-void thrustSelection(Vehicle &State, int t) {
-
-  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 = (t - State.burnStart) / 1000;
-    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
-  }
-
-  else if (abs(State.burnVelocity + State.vz) < 0.001) {
-    // Start burn
-    State.burnStart = t;
-    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 pidController(Vehicle &State, struct Vehicle &PrevState) {
   // Make sure we start reacting when we start burning
   if (State.thrust > 0.01) {
@@ -233,7 +146,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
     State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
 
     // TVC block properly
-
     State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
                   State.Kd * State.d_yError) /
                  State.momentArm;
@@ -242,69 +154,58 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
                  State.momentArm;
 
   } else {
-    State.PIDx = 0;
-    State.PIDy = 0;
+    State.PIDx = 0.0;
+    State.PIDy = 0.0;
   }
 
-  // PID Force limiter X
-  if (State.PIDx > State.thrust)
-    State.PIDx = State.thrust;
-  else if (State.PIDx < -1 * State.thrust)
-    State.PIDx = -1 * State.thrust;
-
-  // PID Force limiter Y
-  if (State.PIDy > State.thrust)
-    State.PIDy = State.thrust;
-  else if (State.PIDy < -1 * State.thrust)
-    State.PIDy = -1 * State.thrust;
+  // PID Force Limiter
+  State.PIDx = limit(State.PIDx, State.thrust, -State.thrust);
+  State.PIDy = limit(State.PIDy, State.thrust, -State.thrust);
 }
 
-void TVC(Vehicle &State) {
+void TVC(Vehicle &State, Vehicle &PrevState) {
   if (State.thrust < 0.1) {
     // Define forces and moments for t = 0
-    State.Fx = 0;
-    State.Fy = 0;
+    State.Fx = 0.0;
+    State.Fy = 0.0;
     State.Fz = g * State.massInitial;
 
-    State.momentX = 0;
-    State.momentY = 0;
-    State.momentZ = 0;
+    State.momentX = 0.0;
+    State.momentY = 0.0;
+    State.momentZ = 0.0;
 
   } else {
     // Convert servo position to degrees for comparison to max allowable
-    State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
+    State.xServoDegs = (180.0 / M_PI) * asin(State.PIDx / State.thrust);
+    State.yServoDegs = (180.0 / M_PI) * asin(State.PIDy / State.thrust);
 
-    // Servo position limiter
-    if (State.xServoDegs > State.maxServo)
-      State.xServoDegs = State.maxServo;
-    else if (State.xServoDegs < -1 * State.maxServo)
-      State.xServoDegs = -1 * State.maxServo;
+    // Limit Servo Position
+    State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
+    State.yServoDegs = limit(State.yServoDegs, State.maxServo, -State.maxServo);
 
-    // Convert servo position to degrees for comparison to max allowable
-    State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
+    // Servo Rate
+    State.xServoDegsDot =
+        derivative(State.xServoDegs, PrevState.xServoDegs, State.stepSize);
+    State.yServoDegsDot =
+        derivative(State.yServoDegs, PrevState.yServoDegs, State.stepSize);
 
-    // Servo position limiter
-    if (State.yServoDegs > State.maxServo)
-      State.yServoDegs = State.maxServo;
-    else if (State.yServoDegs < -1 * State.maxServo)
-      State.yServoDegs = -1 * State.maxServo;
+    // Limit Servo Rate
+    State.xServoDegsDot =
+        limit(State.xServoDegsDot, State.maxServoRate, -State.maxServoRate);
+    State.yServoDegsDot =
+        limit(State.yServoDegsDot, State.maxServoRate, -State.maxServoRate);
 
-    // 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;
+    // Back to Degrees
+    State.xServoDegs =
+        integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize);
+    State.yServoDegs =
+        integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize);
   }
 }
 
-void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
-               int t) {
+void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
+  int t = State.time;
+
   stateVector.x[t] = State.x;
   stateVector.y[t] = State.y;
   stateVector.z[t] = State.z;
@@ -333,71 +234,47 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
   stateVector.PIDx[t] = State.PIDx;
   stateVector.PIDy[t] = State.PIDy;
 
-  // Set "prev" values for next timestep
-  PrevState = State;
-}
-
-void write2CSV(outVector &stateVector, Vehicle &State) {
-
-  // Deleting any previous output file
-  if (remove("simOut.csv") != 0)
-    perror("No file deletion necessary");
-  else
-    puts("Previous output file successfully deleted");
-
-  // Define and open output file "simOut.csv"
-  std::fstream outfile;
-  outfile.open("simOut.csv", std::ios::app);
-
-  // Output file header. These are the variables that we output - useful for
-  // debugging
-  outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
-             "pitchdot, rolldot, Servo1, Servo2, thrustFiring, PIDx, PIDy, "
-             "thrust, deriv"
-          << std::endl;
-
-  // writing to output file
-  for (int t = 0; t < State.simTime; t += State.stepSize) {
-    outfile << t << ", ";
-
-    outfile << stateVector.x[t] << ", ";
-    outfile << stateVector.y[t] << ", ";
-    outfile << stateVector.z[t] << ", ";
-
-    outfile << stateVector.vx[t] << ", ";
-    outfile << stateVector.vy[t] << ", ";
-    outfile << stateVector.vz[t] << ", ";
-
-    outfile << stateVector.ax[t] << ", ";
-    outfile << stateVector.ay[t] << ", ";
-    outfile << stateVector.az[t] << ", ";
-
-    outfile << stateVector.yaw[t] * 180 / M_PI << ", ";
-    outfile << stateVector.pitch[t] * 180 / M_PI << ", ";
-    outfile << stateVector.roll[t] * 180 / M_PI << ", ";
-
-    outfile << stateVector.yawdot[t] * 180 / M_PI << ", ";
-    outfile << stateVector.pitchdot[t] * 180 / M_PI << ", ";
-    outfile << stateVector.rolldot[t] * 180 / M_PI << ", ";
-
-    outfile << stateVector.servo1[t] << ", ";
-    outfile << stateVector.servo2[t] << ", ";
-
-    outfile << stateVector.thrustFiring[t] << ", ";
-
-    outfile << stateVector.PIDx[t] << ", ";
-    outfile << stateVector.PIDy[t] << std::endl;
-  }
-
-  outfile.close();
-  std::cout << "simOut.csv created successfully.\n" << std::endl;
+  stateVector.thrust[t] = State.thrust;
 }
 
 double derivative(double current, double previous, double step) {
-  double dxdt = (current - previous) / (step / 1000);
+  double dxdt = (current - previous) / (step / 1000.0);
   return dxdt;
 }
 
 double integral(double currentChange, double prevValue, double dt) {
-  return (currentChange * dt / 1000) + prevValue;
+  return (currentChange * dt / 1000.0) + prevValue;
+}
+
+double limit(double value, double upr, double lwr) {
+  if (value > upr)
+    value = upr;
+  else if (value < lwr)
+    value = lwr;
+  else
+    value = value;
+
+  return value;
+}
+
+void getThrust(Vehicle &State) {
+  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
+    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.0) -
+                   11.609 * pow(State.burnElapsed, 5.0) +
+                   60.739 * pow(State.burnElapsed, 4.0) -
+                   162.99 * pow(State.burnElapsed, 3.0) +
+                   235.6 * pow(State.burnElapsed, 2.0) -
+                   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 = 2;
+    State.thrust = 0.0;
+  }
 }

include/teensy.h

@@ -0,0 +1,398 @@
+#include "Vehicle.h"
+#include <Arduino.h>
+#include <HX711.h>
+#include <PWMServo.h>
+#include <SD.h>
+#include <SPI.h>
+
+void testGimbal(class PWMServo &, class PWMServo &);
+void initLoadCells(struct Vehicle &);
+void initFile();
+void thrustInfo(struct Vehicle &);
+void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
+void write2CSV(struct Vehicle &);
+void printSimResults(struct Vehicle &);
+void teensyAbort();
+double loadCellFilter(double current, double previous);
+
+// Load cell stuff
+HX711 lc0;
+HX711 lc1;
+HX711 lc2;
+HX711 lc3;
+
+void read_lc0();
+void read_lc1();
+void read_lc2();
+void read_lc3();
+
+const int lc_clock = 23;
+const int pin_lc0 = 14;
+const int pin_lc1 = 15;
+const int pin_lc2 = 19;
+const int pin_lc3 = 20;
+const int pin_igniter = 7;
+
+double lcGain0 = -98.32;
+double lcGain1 = -97.59;
+double lcGain2 = -100.51;
+double lcGain3 = -118.65;
+
+// SD card stuff
+const int chipSelect = BUILTIN_SDCARD;
+File dataFile;
+
+void testGimbal(PWMServo &servo1, PWMServo &servo2) {
+  int servoTest = 90;
+
+  servo1.write(servoTest);
+  servo2.write(servoTest);
+
+  // Servo 1 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(90 + servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(90 + servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 1 & 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(90 + servoTest);
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(90 + servoTest);
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+
+  delay(30);
+  // BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
+  servo1.write(90);
+  servo2.write(90);
+}
+
+void initLoadCells(Vehicle &State) {
+  // Configure clock pin with high impedance to protect
+  // pin (if this doesn't  work, change to OUTPUT)
+  pinMode(lc_clock, INPUT);
+
+  // Configure load cell data pins as inputs
+  pinMode(pin_lc0, INPUT);
+  pinMode(pin_lc1, INPUT);
+  pinMode(pin_lc2, INPUT);
+  pinMode(pin_lc3, INPUT);
+  lc0.begin(pin_lc0, lc_clock);
+  lc1.begin(pin_lc1, lc_clock);
+  lc2.begin(pin_lc2, lc_clock);
+  lc3.begin(pin_lc3, lc_clock);
+
+  Serial.print("Calibrating");
+  lc0.set_scale();
+  lc1.set_scale();
+  lc2.set_scale();
+  lc3.set_scale();
+
+  lc0.tare(50);
+  Serial.print(".");
+  lc1.tare(50);
+  Serial.print(".");
+  lc2.tare(50);
+  Serial.println(".");
+  lc3.tare(50);
+
+  // Attach ISRs to load cell data pins to read data when available
+  attachInterrupt(digitalPinToInterrupt(pin_lc0), read_lc0, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc1), read_lc1, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);
+
+  Serial.println("Load Cells Initialized");
+
+  // Initializes State variables
+  State.lc0 = 9.81 * 0.001 * lc0.get_value() / lcGain0;
+  State.lc1 = 9.81 * 0.001 * lc1.get_value() / lcGain1;
+  State.lc2 = 9.81 * 0.001 * lc2.get_value() / lcGain2;
+  State.lc3 = 9.81 * 0.001 * lc3.get_value() / lcGain3;
+}
+
+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.");
+
+  int i = 1;
+  const char *fileName;
+
+  if (SD.exists("simOut.csv")) {
+    while (i > 0.0) {
+      fileName = ("simOut_" + String(i) + ".csv").c_str();
+
+      if (!SD.exists(fileName)) {
+        // Open simOut_i.csv
+        dataFile = SD.open(fileName, FILE_WRITE);
+
+        if (dataFile) {
+          Serial.println("File successfully opened!");
+
+        } else {
+          // if the file didn't open, print an error:
+          Serial.println("Error opening output file. \n\nABORTING SIMULATION");
+          teensyAbort();
+        }
+        i = 0.0;
+
+      } else {
+        i++;
+      }
+    }
+  } else {
+    // 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 output file. \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,thrust,simResponse,lc0,lc1,lc2,lc3,lc0_"
+      "processed,lc1_processed,"
+      "lc2_processed,lc3_processed");
+}
+
+void thrustInfo(Vehicle &State) {
+  if ((abs(State.burnVelocity + State.vz) < 1.03) &&
+      (State.thrustFiring == 0)) {
+    // Start burn
+    State.burnStart = State.time;
+    State.burnElapsed = 0.0;
+
+    analogWrite(pin_igniter, 256);
+    State.thrustFiring = 1;
+    getThrust(State);
+
+  } else if (State.thrustFiring == 1) {
+    State.burnElapsed = (State.time - State.burnStart) / 1000.0;
+    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
+
+    getThrust(State);
+
+    double r = 3.0;
+    double R = 5.0;
+    // Vector math to aqcuire thrust vector components
+    // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
+    // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
+    State.thrust = State.lc0_processed + State.lc1_processed +
+                   State.lc2_processed + State.lc3_processed;
+    State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
+    State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
+    State.Fz =
+        sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
+        (state.mass * g);
+
+  } else {
+    State.thrust = 0.0;
+  }
+}
+
+void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
+  State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
+  State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
+  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.0;
+
+  State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
+
+  servo1.write(90 + State.xServoDegs);
+  servo2.write(90 + State.yServoDegs);
+}
+
+void write2CSV(Vehicle &State) {
+  dataFile.print(String(State.time, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.x, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.y, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.z, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.vx, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.vy, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.vz, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.ax, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.ay, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.az, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.yaw * 180.0 / M_PI, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.pitch * 180.0 / M_PI, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.roll * 180.0 / M_PI, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.yawdot * 180.0 / M_PI, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.pitchdot * 180.0 / M_PI, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.rolldot * 180.0 / M_PI, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.xServoDegs, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.yServoDegs, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.thrustFiring, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.thrust, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.stepDuration, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.lc0, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc1, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc2, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc3, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.lc0_processed, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc1_processed, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc2_processed, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc3_processed, 5));
+
+  dataFile.print("\n");
+}
+
+void printSimResults(Vehicle &State) {
+  State.yaw = State.yaw * 180.0 / M_PI;
+  State.pitch = State.pitch * 180.0 / M_PI;
+  State.roll = State.roll * 180.0 / M_PI;
+
+  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 < 1.0) {
+    Serial.print("Landing Velocity  < 1.0 m/s      | PASS | ");
+  } else {
+    Serial.print("Landing Velocity  < 1.0 m/s      | FAIL | ");
+  }
+  Serial.print("Final Velocity: [" + String(State.vx) + ", " +
+               String(State.vy) + ", " + String(State.vz) + "]\n");
+
+  Serial.print("\nSimulation Complete\n");
+}
+
+void closeFile() {
+  // close the file:
+  dataFile.close();
+  Serial.println("File closed\n");
+}
+
+void teensyAbort() {
+  while (1) {
+    digitalWrite(BUILTIN_LED, HIGH);
+    delay(1000);
+    digitalWrite(BUILTIN_LED, LOW);
+    delay(1000);
+  }
+}
+
+// ISRs to print data to serial monitor
+void read_lc0() {
+  State.lc0 = lc0.get_value();
+  State.lc0_processed = 0.00981 * State.lc0 / lcGain0;
+  State.lc0_processed =
+      loadCellFilter(State.lc0_processed, PrevState.lc0_processed);
+  PrevState.lc0_processed = State.lc0_processed;
+}
+void read_lc1() {
+  State.lc1 = lc1.get_value();
+  State.lc1_processed = 0.00981 * State.lc1 / lcGain1;
+  State.lc1_processed =
+      loadCellFilter(State.lc1_processed, PrevState.lc1_processed);
+  PrevState.lc1_processed = State.lc1_processed;
+}
+void read_lc2() {
+  State.lc2 = lc2.get_value();
+  State.lc2_processed = 0.00981 * State.lc2 / lcGain2;
+  State.lc2_processed =
+      loadCellFilter(State.lc2_processed, PrevState.lc2_processed);
+  PrevState.lc2_processed = State.lc2_processed;
+}
+void read_lc3() {
+  State.lc3 = lc3.get_value();
+  State.lc3_processed = 0.00981 * State.lc3 / lcGain3;
+  State.lc3_processed =
+      loadCellFilter(State.lc3_processed, PrevState.lc3_processed);
+  PrevState.lc3_processed = State.lc3_processed;
+}
+
+double loadCellFilter(double current, double previous) {
+  if (abs(current - previous > 60.0)) {
+    return previous;
+  } else {
+    return current;
+  }
+}

juliaHelpers/Project.toml

@@ -0,0 +1,5 @@
+[deps]
+CSV = "336ed68f-0bac-5ca0-87d4-7b16caf5d00b"
+DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"
+PlotlyJS = "f0f68f2c-4968-5e81-91da-67840de0976a"
+Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"

juliaHelpers/simplot.jl

@@ -0,0 +1,45 @@
+using Plots
+using CSV
+using DataFrames
+
+theme(:juno)
+plotlyjs()
+
+
+df = CSV.File("./public/simOut.csv") |> DataFrame
+# df = CSV.File("./simOut.csv") |> DataFrame
+
+
+println(describe(df))
+
+p1 = let 
+    a = plot(df.t, [df.ax df.ay df.az], labels = ["ẍ" "ÿ" "z̈"], title = "Acceleration", ylabel = "m/s^2")
+
+    v = plot(df.t, df.vx .* df.vx .+ df.vy .* df.vy .+ df.vz .* df.vz .|> sqrt, labels = "Total Velocity", title = "Total Velocity", ylabel = "m/s", linecolor = "orchid")
+
+    x = plot(df.t, df.z, labels = "Altitude", title = "Altitude", ylabel = "m", xlabel = "time (ms)", linecolor = "chartreuse")
+
+
+    plot(a, v, x, layout = (3, 1), legend = :outertopright)
+end
+
+
+p2 = let
+    ypr = plot(df.t, [df.yaw df.pitch df.roll], label = ["Yaw" "Pitch" "Roll"], ylabel = "Euler Angles (degree)", title = "Vehicle Deflection")
+
+    ω = plot(df.t, [df.yawdot df.pitchdot df.rolldot], label = ["ÿ" "p̈" "r̈"], ylabel = "Angular Velocity (deg/s)", title = "Vehicle Deflection", xlabel = "time (ms)")
+
+    plot(ypr, ω, layout = (2, 1))
+end
+
+p3 = plot(df.t, [df.Servo1 df.Servo2], label = ["Yaw" "Pitch"], ylabel = "Servo Position (degree)", xlabel = "time (ms)", title = "Servo Positions")
+
+
+global i = 1
+theme(:ggplot2)
+for p in [p1 p2 p3]
+    savefig(p, "./public/plot$i.svg")
+    savefig(p, "./public/plot$i.html")
+    savefig(p, "./public/plot$i.png")
+    global i = i + 1
+end

matlabHelpers/simPlot.m

@@ -30,6 +30,7 @@ Servo2      = T(:, 18);
 PIDx      = T(:, 20);
 PIDy      = T(:, 21);
 
+thrust = T(:, 22);
 % Acceleration
 subplot(3, 1, 1)
 plot(t, az)
@@ -51,7 +52,6 @@ title('Altitude vs Time')
 xlabel('Time (s)')
 ylabel('Altitude (m)')
 ylim([0 z(1)+5])
-saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
 
 figure(2)
 
@@ -75,7 +75,6 @@ plot(t, rolldot)
 title('Angular Velocity vs Time')
 xlabel('Time (ms)')
 ylabel('Angular Velocity (deg/s)')
-saveas(gcf,'outputs/Euler Angles vs Time.png')
 legend("yawdot", "pitchdot", "rolldot")
 
 figure(3)
@@ -93,4 +92,12 @@ plot(t, Servo2)
 title('Servo 2 Position vs Time')
 xlabel('Time (ms)')
 ylabel('Servo 2 Position (rad)')
-saveas(gcf,'outputs/Servo Position vs Time.png')
+
+figure(4)
+
+% Servo 1 Position
+
+plot(t, thrust)
+title('Thrust vs Time')
+xlabel('Time (ms)')
+ylabel('Thrust (N)')

platformio.ini

@@ -0,0 +1,20 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:NATIVE]
+platform = native
+build_flags = -std=c++11 -Wall -O1 -D NATIVE
+
+[env:teensy41]
+platform = teensy
+board = teensy41
+framework = arduino
+build_flags = -std=c++11 -Wall -O1 -D TEENSY
+lib_deps = bogde/HX711@^0.7.4

pythonHelpers/plots.py

@@ -4,7 +4,7 @@ import pandas as pd
 
 plt.style.use("ggplot")
 
-df = pd.read_csv("./public/simOut.csv", delimiter=", ")
+df = pd.read_csv("./public/simOut.csv")
 
 
 # Acceleration

src/main.cpp

@@ -1,5 +1,4 @@
-#define M_PI 3.14159265359
-
+#if defined(_WIN32) || defined(NATIVE)
 #include <cmath>
 #include <fstream>
 #include <iostream>
@@ -7,56 +6,134 @@
 #include <stdio.h>
 #include <string>
 #include <vector>
+#elif defined(TEENSY)
+#include <Arduino.h>
+#endif
 
+#define M_PI 3.14159265359
 #include "Vehicle.h"
 #include "sim.h"
 
-bool sim(struct Vehicle &);
-
-int main() {
 Vehicle State;
 Vehicle PrevState;
 
-  // Initial Velocity
-  State.vx = 0; // [m/s]
-  State.vy = 0; // [m/s]
-  State.vz = 0; // [m/s]
+#if defined(TEENSY)
 
-  // Initial YPR
-  State.yaw = 10 * M_PI / 180;  // [rad]
-  State.pitch = 5 * M_PI / 180; // [rad]
-  State.roll = 0 * M_PI / 180;  // [rad]
+int BUILTIN_LED = 13;
+unsigned long last, initTime;
 
-  // Initial YPRdot
-  State.yawdot = 1 * M_PI / 180;    // [rad/s]
-  State.pitchdot = -1 * M_PI / 180; // [rad/s]
-  State.rolldot = 0 * M_PI / 180;   // [rad/s]
+#include "teensy.h"
+#include <HX711.h>
+#include <PWMServo.h>
 
-  // Servo Limitation
-  State.maxServo = 15; // [degs]
+PWMServo servo1;
+PWMServo servo2;
 
-  // Vehicle Properties
-  State.massInitial = 1.2;       // [kg]
-  State.vehicleHeight = 0.5318;  // [m]
-  State.vehicleRadius = 0.05105; // [m]
-  State.momentArm = 0.145;       // [m]
+const int pin_servo1 = 33;
+const int pin_servo2 = 29;
+const int pin_igniter = 7;
 
-  // Sim Step Size
-  State.stepSize = 1; // [ms]
+int servoPos = 90; // variable to store the servo position;
 
-  // Other Properties
-  State.massPropellant = 0.06;                                  // [kg]
-  State.massBurnout = State.massInitial - State.massPropellant; // [kg]
-  State.burntime = 3.45 - 0.148;                                // [s]
-  State.mdot = State.massPropellant / State.burntime;           // [kg/s]
-  State.mass = State.massInitial;                               // [kg]
-  State.burnElapsed = 2000;                                     // [s]
-  PrevState.thrust = 0;                                         // [N]
+void setup() {
+  Serial.begin(9600);
 
-  bool outcome = sim(State, PrevState);
+  delay(5000);
+  Serial.println("Simulation Countdown:");
+  for (int i = 0; i < 10; i++) {
+    Serial.println(10 - i);
+    delay(1000);
+  }
+
+  initLoadCells(State);
+  init_Vehicle(State);
+
+  servo1.attach(pin_servo1);
+  servo2.attach(pin_servo2);
+  testGimbal(servo1, servo2);
+  Serial.println("Servos Tested");
+
+  delay(3000);
+  Serial.println("Simulated Vehicle Initalized");
+
+  // Determine when to burn
+  analogWrite(pin_igniter, 0);
+  burnStartTimeCalc(State);
+  Serial.println("Starting Height Calculated");
+
+  initFile();
+
+  // PLACE BUTTON HERE---------------------------------------------
+
+  initTime = micros();
+}
+
+void loop() {
+  last = micros();
+
+  vehicleDynamics(State, PrevState);
+  thrustInfo(State);
+  pidController(State, PrevState);
+  TVC(State, PrevState);
+  processTVC(State, servo1, servo2);
+
+  write2CSV(State);
+
+  // Set previous values for next timestep
+  PrevState = State;
+  State.time += State.stepSize;
+
+  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+    analogWrite(pin_igniter, 0);
+    Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
+                   " seconds");
+    printSimResults(State);
+
+    closeFile();
+    delay(3000);
+
+    Serial.println("SUCCESS");
+    Serial.println("Exiting Sim");
+    teensyAbort();
+  }
+
+  State.stepDuration = micros() - last;
+  delayMicroseconds((1000.0 * State.stepSize) - State.stepDuration);
+}
+
+#endif
+
+#if defined(_WIN32) || defined(NATIVE)
+
+#include "native.h"
+outVector stateVector;
+
+int main() {
+
+  init_Vehicle(State);
+
+  // Determine when to burn
+  burnStartTimeCalc(State);
+
+  do {
+    vehicleDynamics(State, PrevState);
+    thrustInfo(State);
+    pidController(State, PrevState);
+    TVC(State, PrevState);
+    processTVC(State);
+    state2vec(State, PrevState, stateVector);
+
+    // Set "prev" values for next timestep
+    PrevState = State;
+    State.time += State.stepSize;
+
+    if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+      write2CSV(stateVector, State);
+      printSimResults(State);
+      init_Vehicle(State);
+    }
+  } while ((State.z > 0.0) || (State.thrustFiring != 2));
 
-  std::cout << std::endl << "Simulation Complete 🚀" << std::endl;
-  //                                            ^^^
-  // 50% chance this makes Mattys linux crash
   return 0;
 }
+#endif