From 03c5b6d16f36827f2868e1615d04a02a94280814 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Fri, 12 Nov 2021 15:05:03 -0700
Subject: [PATCH 01/14] printing timing info

---
 include/Vehicle.h |  1 +
 include/teensy.h  | 12 +++++-------
 src/main.cpp      | 13 +++++++++----
 3 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/include/Vehicle.h b/include/Vehicle.h
index 921c953..a5de21e 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -41,6 +41,7 @@ struct Vehicle {
 
   double simTime;
   double stepSize;
+  double stepDuration;
 
   double time = 0.0;
 };
diff --git a/include/teensy.h b/include/teensy.h
index 62d7076..a6ab093 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -75,7 +75,7 @@ void initFile() {
   // 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");
+      "Servo1,Servo2,thrustFiring,thrust,simResponse");
 }
 
 void thrustInfo(Vehicle &State) {
@@ -166,14 +166,12 @@ void write2CSV(Vehicle &State) {
 
   dataFile.print(String(State.thrustFiring, 5));
   dataFile.print(",");
-
-  dataFile.print(String(State.PIDx, 5));
-  dataFile.print(",");
-  dataFile.print(String(State.PIDy, 5));
-  dataFile.print(",");
-
   dataFile.print(String(State.thrust, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.stepDuration, 5));
   dataFile.print("\n");
+
 }
 
 void printSimResults(Vehicle &State) {
diff --git a/src/main.cpp b/src/main.cpp
index 21d20d5..f71edcd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -12,7 +12,7 @@
 #include <Arduino.h>
 
 int BUILTIN_LED = 13;
-unsigned long last;
+unsigned long last, initTime;
 #endif
 
 #include "Vehicle.h"
@@ -51,6 +51,8 @@ void setup() {
   delay(1000);
   initFile();
   delay(1000);
+
+  initTime = micros();
 }
 #endif
 
@@ -76,22 +78,23 @@ void loop() {
 #elif defined(TEENSY)
 void loop() {
 
-  last = millis();
+  last = micros();
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
   processTVC(State);
+
+  State.stepDuration = micros() - last;
   write2CSV(State);
 
   // Set "prev" values for next timestep
   PrevState = State;
-
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     printSimResults(State);
-    Serial.println("Run duration:" + String(millis() - last) + " ms");
+    Serial.println("Run duration:" + String(micros() - initTime) + " us");
 
     closeFile();
     delay(20000);
@@ -100,6 +103,8 @@ void loop() {
     Serial.println("Aborting Sim");
     teensyAbort();
   }
+
+  delay(20 - ((micros() - last) * 1000.0));
 }
 #endif
 

From a6dcb6863fe4626ce3b64662ba4b3276f250c1a4 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Fri, 12 Nov 2021 15:47:27 -0700
Subject: [PATCH 02/14] added load cells to csv

---
 include/teensy.h | 17 +++++++++++++----
 src/main.cpp     | 16 ++++++++--------
 2 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index b2056b9..07402d8 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -7,7 +7,7 @@ double loadCellCalibrate();
 void initFile();
 void thrustInfo(struct Vehicle &);
 void processTVC(struct Vehicle &);
-void write2CSV(struct Vehicle &);
+void write2CSV(struct Vehicle &, double a, double b, double c, double d);
 void printSimResults(struct Vehicle &);
 void teensyAbort();
 
@@ -75,7 +75,7 @@ void initFile() {
   // 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");
+      "Servo1,Servo2,thrustFiring,thrust,simResponse,l0,l1,l2,l3");
 }
 
 void thrustInfo(Vehicle &State) {
@@ -122,7 +122,7 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   State.momentZ = 0.0;
 }
 
-void write2CSV(Vehicle &State) {
+void write2CSV(Vehicle &State, double a, double b, double c, double d) {
   dataFile.print(String(State.time, 5));
   dataFile.print(",");
 
@@ -172,8 +172,17 @@ void write2CSV(Vehicle &State) {
   dataFile.print(",");
 
   dataFile.print(String(State.stepDuration, 5));
-  dataFile.print("\n");
+  dataFile.print(",");
 
+  dataFile.print(String(a, 5));
+  dataFile.print(",");
+  dataFile.print(String(b, 5));
+  dataFile.print(",");
+  dataFile.print(String(c, 5));
+  dataFile.print(",");
+  dataFile.print(String(d, 5));
+
+  dataFile.print("\n");
 }
 
 void printSimResults(Vehicle &State) {
diff --git a/src/main.cpp b/src/main.cpp
index 4382951..d7d3459 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -29,10 +29,10 @@ LoadCells loadCells;
 #include "teensy.h"
 const int lc_clock = 23;
 
-const int lc_data_0 = 0;
-const int lc_data_1 = 1;
-const int lc_data_2 = 2;
-const int lc_data_3 = 3;
+const int lc_data_0 = 14;
+const int lc_data_1 = 15;
+const int lc_data_2 = 19;
+const int lc_data_3 = 20;
 
 #endif
 
@@ -107,17 +107,17 @@ void loop() {
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
-  
+
   processTVC(State, loadCells);
 
   State.stepDuration = micros() - last;
-  write2CSV(State);
+  write2CSV(State, loadCells.lc0Value, loadCells.lc1Value, loadCells.lc2Value,
+            loadCells.lc3Value);
 
   // Set "prev" values for next timestep
   PrevState = State;
-  
-  // state2vec(State, PrevState, stateVector);
 
+  // state2vec(State, PrevState, stateVector);
 
   State.time += State.stepSize;
 

From 069ab2a9e441e8e75511f4a20dc98ad9af754bb3 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Fri, 12 Nov 2021 15:54:34 -0700
Subject: [PATCH 03/14] extra code

---
 include/teensy.h | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index 07402d8..f948298 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -14,15 +14,6 @@ void teensyAbort();
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
-double loadCellCalibrate() {
-  // place code to calibrate load cells in here
-  double loadTotal = 0.0;
-  for (double t = 0.0; t == 10.0; ++t) {
-    loadTotal += 1.0;
-    delay(15);
-  }
-  return loadTotal / 10.0;
-}
 
 void initFile() {
   Serial.print("Initializing SD card...");

From b9c2e7fb3557eef605fde82528f3955268cc0d77 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Sat, 13 Nov 2021 11:17:44 -0700
Subject: [PATCH 04/14] now reading from load cells each loop

---
 include/LoadCells.h | 37 +++++++++++++++++++------------------
 include/teensy.h    |  5 ++---
 src/main.cpp        | 18 ++++++++++++++----
 3 files changed, 35 insertions(+), 25 deletions(-)

diff --git a/include/LoadCells.h b/include/LoadCells.h
index 7aefd5d..2b89a93 100644
--- a/include/LoadCells.h
+++ b/include/LoadCells.h
@@ -2,20 +2,22 @@
 #include <HX711.h>
 
 struct LoadCells {
-  HX711 loadcell_0;
-  HX711 loadcell_1;
-  HX711 loadcell_2;
-  HX711 loadcell_3;
+  HX711 lc0;
+  HX711 lc1;
+  HX711 lc2;
+  HX711 lc3;
 
-  double lc0Value;
-  double lc1Value;
-  double lc2Value;
-  double lc3Value;
+  // Current calibrated value
+  double lc0Val;
+  double lc1Val;
+  double lc2Val;
+  double lc3Val;
 
-  double lc0Calibration;
-  double lc1Calibration;
-  double lc2Calibration;
-  double lc3Calibration;
+  // Calibration offset
+  double lc0Cal;
+  double lc1Cal;
+  double lc2Cal;
+  double lc3Cal;
 };
 
 double loadCellCalibrate(HX711 loadCell) {
@@ -28,10 +30,9 @@ double loadCellCalibrate(HX711 loadCell) {
   return loadTotal / 10.0;
 }
 
-void init_LoadCells(LoadCells &loadCells) {
-
-  loadCells.lc0Calibration = loadCellCalibrate(loadCells.loadcell_0);
-  loadCells.lc1Calibration = loadCellCalibrate(loadCells.loadcell_1);
-  loadCells.lc2Calibration = loadCellCalibrate(loadCells.loadcell_2);
-  loadCells.lc3Calibration = loadCellCalibrate(loadCells.loadcell_3);
+void update_LoadCells(LoadCells &loadCells) {
+  loadCells.lc0Val = loadCells.lc0.read();
+  loadCells.lc1Val = loadCells.lc1.read();
+  loadCells.lc2Val = loadCells.lc2.read();
+  loadCells.lc3Val = loadCells.lc3.read();
 }
\ No newline at end of file
diff --git a/include/teensy.h b/include/teensy.h
index f948298..b04d15c 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -14,7 +14,6 @@ void teensyAbort();
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
-
 void initFile() {
   Serial.print("Initializing SD card...");
 
@@ -102,8 +101,8 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   // Vector math to aqcuire thrust vector components
   // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
   // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
-  State.Fx = loadCells.lc1Value + loadCells.lc2Value;
-  State.Fy = loadCells.lc0Value + loadCells.lc3Value;
+  State.Fx = loadCells.lc1Val + loadCells.lc2Val;
+  State.Fy = loadCells.lc0Val + loadCells.lc3Val;
   State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
              (State.mass * g);
 
diff --git a/src/main.cpp b/src/main.cpp
index 5193263..5e9abd7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -58,6 +58,17 @@ void setup() {
   pinMode(lc_data_1, INPUT);
   pinMode(lc_data_2, INPUT);
   pinMode(lc_data_3, INPUT);
+  loadCells.lc0.begin(lc_data_0, lc_clock);
+  loadCells.lc1.begin(lc_data_1, lc_clock);
+  loadCells.lc2.begin(lc_data_2, lc_clock);
+  loadCells.lc3.begin(lc_data_3, lc_clock);
+  Serial.println("Load Cells Initialized");
+
+  loadCells.lc0Cal = loadCellCalibrate(loadCells.lc0);
+  loadCells.lc1Cal = loadCellCalibrate(loadCells.lc1);
+  loadCells.lc2Cal = loadCellCalibrate(loadCells.lc2);
+  loadCells.lc3Cal = loadCellCalibrate(loadCells.lc3);
+  Serial.println("Load Cells Calibrated");
 
   delay(1000);
   init_Vehicle(State);
@@ -69,8 +80,6 @@ void setup() {
   Serial.println("Starting Height Calculated");
   delay(1000);
 
-  init_LoadCells(loadCells);
-
   Serial.println("Load Cells Calibrated");
   delay(1000);
   initFile();
@@ -103,6 +112,7 @@ void loop() {
 void loop() {
 
   last = micros();
+  update_LoadCells(loadCells);
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
@@ -111,8 +121,8 @@ void loop() {
   processTVC(State, loadCells);
 
   State.stepDuration = micros() - last;
-  write2CSV(State, loadCells.lc0Value, loadCells.lc1Value, loadCells.lc2Value,
-            loadCells.lc3Value);
+  write2CSV(State, loadCells.lc0Val, loadCells.lc1Val, loadCells.lc2Val,
+            loadCells.lc3Val);
 
   // Set "prev" values for next timestep
   PrevState = State;

From 47e8042a574e9f53eb10c96a187d056a77f07d76 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Sat, 13 Nov 2021 11:25:44 -0700
Subject: [PATCH 05/14] cleaned write2csv

---
 include/LoadCells.h |  1 +
 include/teensy.h    | 13 +++++++------
 src/main.cpp        |  3 +--
 3 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/include/LoadCells.h b/include/LoadCells.h
index 2b89a93..a47bee6 100644
--- a/include/LoadCells.h
+++ b/include/LoadCells.h
@@ -1,5 +1,6 @@
 
 #include <HX711.h>
+#pragma once
 
 struct LoadCells {
   HX711 lc0;
diff --git a/include/teensy.h b/include/teensy.h
index b04d15c..856dc21 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -1,3 +1,4 @@
+#include "LoadCells.h"
 #include "Vehicle.h"
 #include <Arduino.h>
 #include <SD.h>
@@ -65,7 +66,7 @@ void initFile() {
   // 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,l0,l1,l2,l3");
+      "Servo1,Servo2,thrustFiring,thrust,simResponse,lc0,lc1,lc2,lc3");
 }
 
 void thrustInfo(Vehicle &State) {
@@ -112,7 +113,7 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   State.momentZ = 0.0;
 }
 
-void write2CSV(Vehicle &State, double a, double b, double c, double d) {
+void write2CSV(Vehicle &State, LoadCells &loadCells) {
   dataFile.print(String(State.time, 5));
   dataFile.print(",");
 
@@ -164,13 +165,13 @@ void write2CSV(Vehicle &State, double a, double b, double c, double d) {
   dataFile.print(String(State.stepDuration, 5));
   dataFile.print(",");
 
-  dataFile.print(String(a, 5));
+  dataFile.print(String(loadCells.lc0Val, 5));
   dataFile.print(",");
-  dataFile.print(String(b, 5));
+  dataFile.print(String(loadCells.lc1Val, 5));
   dataFile.print(",");
-  dataFile.print(String(c, 5));
+  dataFile.print(String(loadCells.lc2Val, 5));
   dataFile.print(",");
-  dataFile.print(String(d, 5));
+  dataFile.print(String(loadCells.lc3Val, 5));
 
   dataFile.print("\n");
 }
diff --git a/src/main.cpp b/src/main.cpp
index 5e9abd7..d4828fe 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -121,8 +121,7 @@ void loop() {
   processTVC(State, loadCells);
 
   State.stepDuration = micros() - last;
-  write2CSV(State, loadCells.lc0Val, loadCells.lc1Val, loadCells.lc2Val,
-            loadCells.lc3Val);
+  write2CSV(State, loadCells);
 
   // Set "prev" values for next timestep
   PrevState = State;

From 717bb9267891b870b09321fd4bb2af38b276f218 Mon Sep 17 00:00:00 2001
From: Matthew Robinaugh <robinaum@my.erau.edu>
Date: Sat, 13 Nov 2021 11:32:42 -0700
Subject: [PATCH 06/14] Update lc values to be calibrated and vector math

---
 include/LoadCells.h |  8 ++++----
 include/Vehicle.h   |  2 +-
 include/teensy.h    | 13 ++++++++-----
 3 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/include/LoadCells.h b/include/LoadCells.h
index 2b89a93..02dcc09 100644
--- a/include/LoadCells.h
+++ b/include/LoadCells.h
@@ -31,8 +31,8 @@ double loadCellCalibrate(HX711 loadCell) {
 }
 
 void update_LoadCells(LoadCells &loadCells) {
-  loadCells.lc0Val = loadCells.lc0.read();
-  loadCells.lc1Val = loadCells.lc1.read();
-  loadCells.lc2Val = loadCells.lc2.read();
-  loadCells.lc3Val = loadCells.lc3.read();
+  loadCells.lc0Val = loadCells.lc0.read() - loadCells.lc0Cal;
+  loadCells.lc1Val = loadCells.lc1.read() - loadCells.lc1Cal;
+  loadCells.lc2Val = loadCells.lc2.read() - loadCells.lc2Cal;
+  loadCells.lc3Val = loadCells.lc3.read() - loadCells.lc3Cal;
 }
\ No newline at end of file
diff --git a/include/Vehicle.h b/include/Vehicle.h
index a5de21e..03db381 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -22,7 +22,7 @@ struct Vehicle {
   double thrust, burnElapsed, burnStart;
   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;
diff --git a/include/teensy.h b/include/teensy.h
index b04d15c..ad34b43 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -97,19 +97,22 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   if (State.time == 0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
-
+  double r = 3.0;
+  double R = 5.0;
   // Vector math to aqcuire thrust vector components
   // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
   // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
-  State.Fx = loadCells.lc1Val + loadCells.lc2Val;
-  State.Fy = loadCells.lc0Val + loadCells.lc3Val;
-  State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
-             (State.mass * g);
+  State.Fx = (loadCells.lc1Val - loadCells.lc2Val) * r / R;
+  State.Fy = (loadCells.lc0Val - loadCells.lc3Val) * r / R;
+  State.Fz =
+      loadCells.lc0Val + loadCells.lc1Val + loadCells.lc2Val + loadCells.lc3Val;
 
   // 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));
 }
 
 void write2CSV(Vehicle &State, double a, double b, double c, double d) {

From 812f62f22a6da976b65614547585b92a2b234464 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Sat, 13 Nov 2021 14:48:12 -0700
Subject: [PATCH 07/14] load cell integration

---
 include/LoadCells.h | 39 --------------------------
 include/Vehicle.h   |  2 ++
 include/teensy.h    | 27 ++++++++++--------
 src/main.cpp        | 67 +++++++++++++++++++++++++++++++++------------
 4 files changed, 67 insertions(+), 68 deletions(-)
 delete mode 100644 include/LoadCells.h

diff --git a/include/LoadCells.h b/include/LoadCells.h
deleted file mode 100644
index 318f512..0000000
--- a/include/LoadCells.h
+++ /dev/null
@@ -1,39 +0,0 @@
-
-#include <HX711.h>
-#pragma once
-
-struct LoadCells {
-  HX711 lc0;
-  HX711 lc1;
-  HX711 lc2;
-  HX711 lc3;
-
-  // Current calibrated value
-  double lc0Val;
-  double lc1Val;
-  double lc2Val;
-  double lc3Val;
-
-  // Calibration offset
-  double lc0Cal;
-  double lc1Cal;
-  double lc2Cal;
-  double lc3Cal;
-};
-
-double loadCellCalibrate(HX711 loadCell) {
-  // place code to calibrate load cells in here
-  double loadTotal = 0.0;
-  for (int t = 0; t == 10; ++t) {
-    loadTotal += loadCell.read();
-    delay(15);
-  }
-  return loadTotal / 10.0;
-}
-
-void update_LoadCells(LoadCells &loadCells) {
-  loadCells.lc0Val = loadCells.lc0.read() - loadCells.lc0Cal;
-  loadCells.lc1Val = loadCells.lc1.read() - loadCells.lc1Cal;
-  loadCells.lc2Val = loadCells.lc2.read() - loadCells.lc2Cal;
-  loadCells.lc3Val = loadCells.lc3.read() - loadCells.lc3Cal;
-}
\ No newline at end of file
diff --git a/include/Vehicle.h b/include/Vehicle.h
index 03db381..2c37635 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -44,6 +44,8 @@ struct Vehicle {
   double stepDuration;
 
   double time = 0.0;
+
+  double lc0, lc1, lc2, lc3;
 };
 
 void init_Vehicle(Vehicle &State) {
diff --git a/include/teensy.h b/include/teensy.h
index e987d8c..8e7d600 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -1,4 +1,3 @@
-#include "LoadCells.h"
 #include "Vehicle.h"
 #include <Arduino.h>
 #include <SD.h>
@@ -8,7 +7,7 @@ double loadCellCalibrate();
 void initFile();
 void thrustInfo(struct Vehicle &);
 void processTVC(struct Vehicle &);
-void write2CSV(struct Vehicle &, double a, double b, double c, double d);
+void write2CSV(struct Vehicle &);
 void printSimResults(struct Vehicle &);
 void teensyAbort();
 
@@ -94,7 +93,7 @@ void thrustInfo(Vehicle &State) {
   }
 }
 
-void processTVC(Vehicle &State, LoadCells &loadCells) {
+void processTVC(Vehicle &State) {
   if (State.time == 0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
@@ -103,10 +102,14 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   // Vector math to aqcuire thrust vector components
   // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
   // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
-  State.Fx = (loadCells.lc1Val - loadCells.lc2Val) * r / R;
-  State.Fy = (loadCells.lc0Val - loadCells.lc3Val) * r / R;
-  State.Fz =
-      loadCells.lc0Val + loadCells.lc1Val + loadCells.lc2Val + loadCells.lc3Val;
+  State.Fx = (State.lc1 - State.lc2) * r / R;
+  State.Fy = (State.lc0 - State.lc3) * r / R;
+  State.Fz = State.lc0 + State.lc1 + State.lc2 + State.lc3;
+
+  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;
@@ -116,7 +119,7 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
 }
 
-void write2CSV(Vehicle &State, LoadCells &loadCells) {
+void write2CSV(Vehicle &State) {
   dataFile.print(String(State.time, 5));
   dataFile.print(",");
 
@@ -168,13 +171,13 @@ void write2CSV(Vehicle &State, LoadCells &loadCells) {
   dataFile.print(String(State.stepDuration, 5));
   dataFile.print(",");
 
-  dataFile.print(String(loadCells.lc0Val, 5));
+  dataFile.print(String(State.lc0, 5));
   dataFile.print(",");
-  dataFile.print(String(loadCells.lc1Val, 5));
+  dataFile.print(String(State.lc1, 5));
   dataFile.print(",");
-  dataFile.print(String(loadCells.lc2Val, 5));
+  dataFile.print(String(State.lc2, 5));
   dataFile.print(",");
-  dataFile.print(String(loadCells.lc3Val, 5));
+  dataFile.print(String(State.lc3, 5));
 
   dataFile.print("\n");
 }
diff --git a/src/main.cpp b/src/main.cpp
index d4828fe..f613ae5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -23,10 +23,8 @@ unsigned long last, initTime;
 outVector stateVector;
 #elif defined(TEENSY)
 
-#include "LoadCells.h"
-LoadCells loadCells;
-
 #include "teensy.h"
+#include <HX711.h>
 const int lc_clock = 23;
 
 const int lc_data_0 = 14;
@@ -34,6 +32,16 @@ const int lc_data_1 = 15;
 const int lc_data_2 = 19;
 const int lc_data_3 = 20;
 
+void read_lc0();
+void read_lc1();
+void read_lc2();
+void read_lc3();
+
+HX711 lc0;
+HX711 lc1;
+HX711 lc2;
+HX711 lc3;
+
 #endif
 
 Vehicle State;
@@ -58,20 +66,37 @@ void setup() {
   pinMode(lc_data_1, INPUT);
   pinMode(lc_data_2, INPUT);
   pinMode(lc_data_3, INPUT);
-  loadCells.lc0.begin(lc_data_0, lc_clock);
-  loadCells.lc1.begin(lc_data_1, lc_clock);
-  loadCells.lc2.begin(lc_data_2, lc_clock);
-  loadCells.lc3.begin(lc_data_3, lc_clock);
+  lc0.begin(lc_data_0, lc_clock);
+  lc1.begin(lc_data_1, lc_clock);
+  lc2.begin(lc_data_2, lc_clock);
+  lc3.begin(lc_data_3, lc_clock);
+
+  // Attach ISRs to load cell data pins to read data when available
+  attachInterrupt(digitalPinToInterrupt(lc_data_0), read_lc0, LOW);
+  attachInterrupt(digitalPinToInterrupt(lc_data_1), read_lc1, LOW);
+  attachInterrupt(digitalPinToInterrupt(lc_data_2), read_lc2, LOW);
+  attachInterrupt(digitalPinToInterrupt(lc_data_3), read_lc3, LOW);
+
   Serial.println("Load Cells Initialized");
 
-  loadCells.lc0Cal = loadCellCalibrate(loadCells.lc0);
-  loadCells.lc1Cal = loadCellCalibrate(loadCells.lc1);
-  loadCells.lc2Cal = loadCellCalibrate(loadCells.lc2);
-  loadCells.lc3Cal = loadCellCalibrate(loadCells.lc3);
+  Serial.print("Calibrating");
+  lc0.tare();
+  Serial.print(".");
+  lc1.tare();
+  Serial.print(".");
+  lc2.tare();
+  Serial.print(".");
+  lc3.tare();
+  Serial.println(".");
   Serial.println("Load Cells Calibrated");
 
   delay(1000);
   init_Vehicle(State);
+  State.lc0 = lc0.get_value();
+  State.lc1 = lc1.get_value();
+  State.lc2 = lc2.get_value();
+  State.lc3 = lc3.get_value();
+
   Serial.println("Simulated Vehicle Initalized");
   delay(1000);
 
@@ -80,8 +105,6 @@ void setup() {
   Serial.println("Starting Height Calculated");
   delay(1000);
 
-  Serial.println("Load Cells Calibrated");
-  delay(1000);
   initFile();
   delay(1000);
 
@@ -112,16 +135,17 @@ void loop() {
 void loop() {
 
   last = micros();
-  update_LoadCells(loadCells);
+  Serial.println(State.time);
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
 
-  processTVC(State, loadCells);
+  processTVC(State);
+  delay(2);
 
   State.stepDuration = micros() - last;
-  write2CSV(State, loadCells);
+  write2CSV(State);
 
   // Set "prev" values for next timestep
   PrevState = State;
@@ -156,4 +180,13 @@ int main() {
 
   return 0;
 }
-#endif
\ No newline at end of file
+#endif
+
+// ISRs to print data to serial monitor
+void read_lc0() { State.lc0 = lc0.get_value(); }
+
+void read_lc1() { State.lc1 = lc1.get_value(); }
+
+void read_lc2() { State.lc2 = lc2.get_value(); }
+
+void read_lc3() { State.lc3 = lc3.get_value(); }

From 0f3e267af8e15a861b1157c28db4b0749509b17a Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Sun, 14 Nov 2021 10:32:04 -0700
Subject: [PATCH 08/14] organized main

---
 src/main.cpp | 184 ++++++++++++++++++++++++---------------------------
 1 file changed, 87 insertions(+), 97 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index f613ae5..a29c03c 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,6 +1,4 @@
-#define M_PI 3.14159265359
-
-#if defined(NATIVE) || defined(_WIN32)
+#if defined(_WIN32) || defined(NATIVE)
 #include <cmath>
 #include <fstream>
 #include <iostream>
@@ -10,28 +8,31 @@
 #include <vector>
 #elif defined(TEENSY)
 #include <Arduino.h>
-
-int BUILTIN_LED = 13;
-unsigned long last, initTime;
 #endif
 
+#define M_PI 3.14159265359
+
 #include "Vehicle.h"
 #include "sim.h"
 
-#if defined(NATIVE) || defined(_WIN32)
-#include "native.h"
-outVector stateVector;
-#elif defined(TEENSY)
+Vehicle State;
+Vehicle PrevState;
+
+#if defined(TEENSY)
+
+int BUILTIN_LED = 13;
+unsigned long last, initTime;
 
 #include "teensy.h"
 #include <HX711.h>
 const int lc_clock = 23;
 
-const int lc_data_0 = 14;
-const int lc_data_1 = 15;
-const int lc_data_2 = 19;
-const int lc_data_3 = 20;
+const int pin_lc0 = 14;
+const int pin_lc1 = 15;
+const int pin_lc2 = 19;
+const int pin_lc3 = 20;
 
+void init_loadCells();
 void read_lc0();
 void read_lc1();
 void read_lc2();
@@ -42,96 +43,30 @@ HX711 lc1;
 HX711 lc2;
 HX711 lc3;
 
-#endif
-
-Vehicle State;
-Vehicle PrevState;
-
-#if defined(NATIVE) || defined(_WIN32)
-void setup() {
-  init_Vehicle(State);
-
-  // Determine when to burn
-  burnStartTimeCalc(State);
-}
-#elif defined(TEENSY)
 void setup() {
   Serial.begin(9600);
 
-  // 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(lc_data_0, INPUT);
-  pinMode(lc_data_1, INPUT);
-  pinMode(lc_data_2, INPUT);
-  pinMode(lc_data_3, INPUT);
-  lc0.begin(lc_data_0, lc_clock);
-  lc1.begin(lc_data_1, lc_clock);
-  lc2.begin(lc_data_2, lc_clock);
-  lc3.begin(lc_data_3, lc_clock);
+  delay(5000);
+  Serial.println("Simulation Countdown:") for (int i = 0; i < 15; i++) {
+    Serial.println(15 - i) delay(1000);
+  }
 
-  // Attach ISRs to load cell data pins to read data when available
-  attachInterrupt(digitalPinToInterrupt(lc_data_0), read_lc0, LOW);
-  attachInterrupt(digitalPinToInterrupt(lc_data_1), read_lc1, LOW);
-  attachInterrupt(digitalPinToInterrupt(lc_data_2), read_lc2, LOW);
-  attachInterrupt(digitalPinToInterrupt(lc_data_3), read_lc3, LOW);
-
-  Serial.println("Load Cells Initialized");
-
-  Serial.print("Calibrating");
-  lc0.tare();
-  Serial.print(".");
-  lc1.tare();
-  Serial.print(".");
-  lc2.tare();
-  Serial.print(".");
-  lc3.tare();
-  Serial.println(".");
-  Serial.println("Load Cells Calibrated");
-
-  delay(1000);
+  init_loadCells();
   init_Vehicle(State);
   State.lc0 = lc0.get_value();
   State.lc1 = lc1.get_value();
   State.lc2 = lc2.get_value();
   State.lc3 = lc3.get_value();
-
   Serial.println("Simulated Vehicle Initalized");
-  delay(1000);
 
   // Determine when to burn
   burnStartTimeCalc(State);
   Serial.println("Starting Height Calculated");
-  delay(1000);
 
   initFile();
-  delay(1000);
 
   initTime = micros();
 }
-#endif
-
-#if defined(NATIVE) || defined(_WIN32)
-void loop() {
-  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);
-  }
-}
-#elif defined(TEENSY)
 void loop() {
 
   last = micros();
@@ -147,10 +82,11 @@ void loop() {
   State.stepDuration = micros() - last;
   write2CSV(State);
 
+  //       \/  Comments are fee you can just write previous bro
   // Set "prev" values for next timestep
   PrevState = State;
 
-  // state2vec(State, PrevState, stateVector);
+  // state2vec(State, PrevState, stateVector); <-- can we delete?
 
   State.time += State.stepSize;
 
@@ -167,26 +103,80 @@ void loop() {
 
   delay(20 - ((micros() - last) * 1000.0));
 }
+
+void init_loadCells() {
+  // 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);
+
+  // 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");
+
+  Serial.print("Calibrating");
+  lc0.tare();
+  Serial.print(".");
+  lc1.tare();
+  Serial.print(".");
+  lc2.tare();
+  Serial.print(".");
+  lc3.tare();
+  Serial.println(".");
+  Serial.println("Load Cells Calibrated");
+}
+
+// ISRs to print data to serial monitor
+void read_lc0() { State.lc0 = lc0.get_value(); }
+void read_lc1() { State.lc1 = lc1.get_value(); }
+void read_lc2() { State.lc2 = lc2.get_value(); }
+void read_lc3() { State.lc3 = lc3.get_value(); }
 #endif
 
 #if defined(_WIN32) || defined(NATIVE)
+
+#include "native.h"
+outVector stateVector;
+
 int main() {
 
-  setup();
+  init_Vehicle(State);
+
+  // Determine when to burn
+  burnStartTimeCalc(State);
 
   do {
-    loop();
+    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));
 
   return 0;
 }
 #endif
-
-// ISRs to print data to serial monitor
-void read_lc0() { State.lc0 = lc0.get_value(); }
-
-void read_lc1() { State.lc1 = lc1.get_value(); }
-
-void read_lc2() { State.lc2 = lc2.get_value(); }
-
-void read_lc3() { State.lc3 = lc3.get_value(); }

From d487d6d30aaff7152cdc65f2a94adf8162739518 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Sun, 14 Nov 2021 10:33:16 -0700
Subject: [PATCH 09/14] should make sure code compiles before pushing

---
 src/main.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index a29c03c..20bb4ba 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -47,8 +47,10 @@ void setup() {
   Serial.begin(9600);
 
   delay(5000);
-  Serial.println("Simulation Countdown:") for (int i = 0; i < 15; i++) {
-    Serial.println(15 - i) delay(1000);
+  Serial.println("Simulation Countdown:");
+  for (int i = 0; i < 15; i++) {
+    Serial.println(15 - i);
+    delay(1000);
   }
 
   init_loadCells();

From 8fa9dba55ec5755f04e67c16f79dfc0a5839554f Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Sun, 14 Nov 2021 13:59:51 -0700
Subject: [PATCH 10/14] Integrated servos, just neefds to be slowed down now

---
 include/teensy.h |  8 ++++++--
 src/main.cpp     | 30 ++++++++++++++++++++----------
 2 files changed, 26 insertions(+), 12 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index 8e7d600..d3e2895 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -1,12 +1,13 @@
 #include "Vehicle.h"
 #include <Arduino.h>
+#include <PWMServo.h>
 #include <SD.h>
 #include <SPI.h>
 
 double loadCellCalibrate();
 void initFile();
 void thrustInfo(struct Vehicle &);
-void processTVC(struct Vehicle &);
+void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
 void write2CSV(struct Vehicle &);
 void printSimResults(struct Vehicle &);
 void teensyAbort();
@@ -93,7 +94,7 @@ void thrustInfo(Vehicle &State) {
   }
 }
 
-void processTVC(Vehicle &State) {
+void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
   if (State.time == 0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
@@ -117,6 +118,9 @@ void processTVC(Vehicle &State) {
   State.momentZ = 0.0;
 
   State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
+
+  servo1.write(State.xServoDegs);
+  servo2.write(State.yServoDegs);
 }
 
 void write2CSV(Vehicle &State) {
diff --git a/src/main.cpp b/src/main.cpp
index 20bb4ba..00105e0 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -25,6 +25,8 @@ unsigned long last, initTime;
 
 #include "teensy.h"
 #include <HX711.h>
+#include <PWMServo.h>
+
 const int lc_clock = 23;
 
 const int pin_lc0 = 14;
@@ -43,6 +45,14 @@ HX711 lc1;
 HX711 lc2;
 HX711 lc3;
 
+PWMServo servo1;
+PWMServo servo2;
+
+const int pin_servo1 = 33;
+const int pin_servo2 = 29;
+
+int servoPos = 0; // variable to store the servo position;
+
 void setup() {
   Serial.begin(9600);
 
@@ -59,6 +69,12 @@ void setup() {
   State.lc1 = lc1.get_value();
   State.lc2 = lc2.get_value();
   State.lc3 = lc3.get_value();
+
+  servo1.attach(pin_servo1);
+  servo2.attach(pin_servo2);
+  servo1.write(0);
+  servo2.write(0);
+
   Serial.println("Simulated Vehicle Initalized");
 
   // Determine when to burn
@@ -69,27 +85,21 @@ void setup() {
 
   initTime = micros();
 }
-void loop() {
 
+void loop() {
   last = micros();
-  Serial.println(State.time);
+
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
-
-  processTVC(State);
-  delay(2);
+  processTVC(State, servo1, servo2);
 
   State.stepDuration = micros() - last;
   write2CSV(State);
 
-  //       \/  Comments are fee you can just write previous bro
-  // Set "prev" values for next timestep
+  // Set previous values for next timestep
   PrevState = State;
-
-  // state2vec(State, PrevState, stateVector); <-- can we delete?
-
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {

From 82e43b1fe88b57901ae12fb59a8098ac5123d902 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Sun, 14 Nov 2021 14:23:28 -0700
Subject: [PATCH 11/14] Added test gimbal function

---
 include/teensy.h | 35 +++++++++++++++++++++++++++++++++++
 src/main.cpp     |  9 +++++----
 2 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index d3e2895..480711f 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -4,6 +4,7 @@
 #include <SD.h>
 #include <SPI.h>
 
+void testGimbal(class PWMServo &, class PWMServo &);
 double loadCellCalibrate();
 void initFile();
 void thrustInfo(struct Vehicle &);
@@ -15,6 +16,40 @@ void teensyAbort();
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
+void testGimbal(PWMServo &servo1, PWMServo &servo2) {
+  int servoTest = 0;
+  ;
+
+  servo1.write(servoTest);
+  servo2.write(servoTest);
+
+  // Servo 1 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo2.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo2.write(servoTest);
+    delay(30);
+  }
+
+  delay(30);
+  servo1.write(0);
+  servo2.write(0);
+}
+
 void initFile() {
   Serial.print("Initializing SD card...");
 
diff --git a/src/main.cpp b/src/main.cpp
index 00105e0..47171ae 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -58,8 +58,8 @@ void setup() {
 
   delay(5000);
   Serial.println("Simulation Countdown:");
-  for (int i = 0; i < 15; i++) {
-    Serial.println(15 - i);
+  for (int i = 0; i < 10; i++) {
+    Serial.println(10 - i);
     delay(1000);
   }
 
@@ -72,9 +72,10 @@ void setup() {
 
   servo1.attach(pin_servo1);
   servo2.attach(pin_servo2);
-  servo1.write(0);
-  servo2.write(0);
+  testGimbal(servo1, servo2);
+  Serial.println("Servos Tested");
 
+  delay(1000);
   Serial.println("Simulated Vehicle Initalized");
 
   // Determine when to burn

From e6f7e4c160cc91ccf2c2d03400dc12a43281a183 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Sun, 14 Nov 2021 16:41:25 -0700
Subject: [PATCH 12/14] Formal build for Avionics Integration Test

---
 include/Vehicle.h | 2 +-
 include/teensy.h  | 6 +++---
 src/main.cpp      | 7 ++++---
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/include/Vehicle.h b/include/Vehicle.h
index 2c37635..ac8b24c 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -80,7 +80,7 @@ void init_Vehicle(Vehicle &State) {
   State.momentArm = 0.145;       // [m]
 
   // Sim Step Size
-  State.stepSize = 1.0; // [ms]
+  State.stepSize = 20.0; // [ms]
 
   // Other Properties
   State.massPropellant = 0.06;                                  // [kg]
diff --git a/include/teensy.h b/include/teensy.h
index 480711f..e57e6c0 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -240,10 +240,10 @@ void printSimResults(Vehicle &State) {
   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 | ");
+  if (landing_velocity < 1.0) {
+    Serial.print("Landing Velocity  < 1.0 m/s      | PASS | ");
   } else {
-    Serial.print("Landing Velocity  < 0.5 m/s      | FAIL | ");
+    Serial.print("Landing Velocity  < 1.0 m/s      | FAIL | ");
   }
   Serial.print("Final Velocity: [" + String(State.vx) + ", " +
                String(State.vy) + ", " + String(State.vz) + "]\n");
diff --git a/src/main.cpp b/src/main.cpp
index 47171ae..f55f9e4 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -75,7 +75,7 @@ void setup() {
   testGimbal(servo1, servo2);
   Serial.println("Servos Tested");
 
-  delay(1000);
+  delay(3000);
   Serial.println("Simulated Vehicle Initalized");
 
   // Determine when to burn
@@ -104,17 +104,18 @@ void loop() {
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+    printSimResults(State);
     Serial.println("Run duration:" + String(micros() - initTime) + " us");
 
     closeFile();
-    delay(20000);
+    delay(3000);
 
     Serial.println("SUCCESS");
     Serial.println("Aborting Sim");
     teensyAbort();
   }
 
-  delay(20 - ((micros() - last) * 1000.0));
+  delay(20.0 - ((micros() - last) / 1000.0));
 }
 
 void init_loadCells() {

From 5a995087616d8c05f9409feeebfa80441d66ff95 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Sun, 14 Nov 2021 17:10:58 -0700
Subject: [PATCH 13/14] More robust gimbal test function, delay based on step
 size of Sim

---
 include/Vehicle.h |  2 +-
 include/teensy.h  | 15 ++++++++++++++-
 src/main.cpp      |  7 ++++---
 3 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/include/Vehicle.h b/include/Vehicle.h
index ac8b24c..ed47313 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -80,7 +80,7 @@ void init_Vehicle(Vehicle &State) {
   State.momentArm = 0.145;       // [m]
 
   // Sim Step Size
-  State.stepSize = 20.0; // [ms]
+  State.stepSize = 5.0; // [ms]
 
   // Other Properties
   State.massPropellant = 0.06;                                  // [kg]
diff --git a/include/teensy.h b/include/teensy.h
index e57e6c0..0ad10f4 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -18,7 +18,6 @@ File dataFile;
 
 void testGimbal(PWMServo &servo1, PWMServo &servo2) {
   int servoTest = 0;
-  ;
 
   servo1.write(servoTest);
   servo2.write(servoTest);
@@ -45,6 +44,20 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
     delay(30);
   }
 
+  delay(1000);
+
+  // Servo 1 & 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(servoTest);
+    servo2.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(servoTest);
+    servo2.write(servoTest);
+    delay(30);
+  }
+
   delay(30);
   servo1.write(0);
   servo2.write(0);
diff --git a/src/main.cpp b/src/main.cpp
index f55f9e4..dbc46fb 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -105,17 +105,18 @@ void loop() {
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     printSimResults(State);
-    Serial.println("Run duration:" + String(micros() - initTime) + " us");
+    Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
+                   " seconds");
 
     closeFile();
     delay(3000);
 
     Serial.println("SUCCESS");
-    Serial.println("Aborting Sim");
+    Serial.println("Exiting Sim");
     teensyAbort();
   }
 
-  delay(20.0 - ((micros() - last) / 1000.0));
+  delay(State.stepSize - ((micros() - last) / 1000.0));
 }
 
 void init_loadCells() {

From 8528b028e507ae0a226ef398003c1107da260de1 Mon Sep 17 00:00:00 2001
From: Anson <anson@ansonbiggs.com>
Date: Sun, 14 Nov 2021 17:13:49 -0700
Subject: [PATCH 14/14] keep platforms the same

---
 include/native.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/include/native.h b/include/native.h
index d12f87a..1cd18fa 100644
--- a/include/native.h
+++ b/include/native.h
@@ -117,10 +117,10 @@ void printSimResults(Vehicle &State) {
   std::cout << "Final Angles:   [" << State.yaw << ", " << State.pitch << "]"
             << std::endl;
 
-  if (landing_velocity < 0.5) {
-    std::cout << "Landing Velocity  < 0.5 m/s      | PASS | ";
+  if (landing_velocity < 1.0) {
+    std::cout << "Landing Velocity  < 1.0 m/s      | PASS | ";
   } else {
-    std::cout << "Landing Velocity  < 0.5 m/s      | FAIL | ";
+    std::cout << "Landing Velocity  < 1.0 m/s      | FAIL | ";
   }
   std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
             << State.vz << "]" << std::endl;