From 1e4fbe8b209f9990794d7d9ede0e11faeb78b077 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Tue, 30 Nov 2021 13:14:12 -0700
Subject: [PATCH] new load cell gains, conditions for ignition while loop

---
 include/teensy.h | 27 +++++++++++++++++++--------
 src/main.cpp     | 10 ++++++++--
 2 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index 9b3415b..0925f41 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -35,10 +35,10 @@ const int pin_lc2 = 19; // White
 const int pin_lc3 = 20; // Black
 const int pin_igniter = 7;
 
-double lcGain0 = -107.39;
-double lcGain1 = -107.39;
-double lcGain2 = -107.39;
-double lcGain3 = -107.39;
+double lcGain0 = -300.72;
+double lcGain1 = -367.5;
+double lcGain2 = -565.78;
+double lcGain3 = -505.02;
 
 // SD card stuff
 const int chipSelect = BUILTIN_SDCARD;
@@ -238,10 +238,13 @@ void thrustInfo(Vehicle &State) {
     digitalWrite(pin_igniter, HIGH);
 
     // Wait for motor to start burning
-    Serial.println("waiting for motor to start.");
-    while (lc_val * 1.5 < State.lc0_processed + State.lc1_processed +
-                              State.lc2_processed + State.lc3_processed) {
-      ;
+    Serial.println("Waiting for motor to start.");
+    while ((lc_val * 10) > (State.lc0_processed + State.lc1_processed +
+                            State.lc2_processed + State.lc3_processed)) {
+      // Serial.print(lc_val * 10);
+      // Serial.print(", ");
+      // Serial.println(State.lc0_processed + State.lc1_processed +
+      //               State.lc2_processed + State.lc3_processed);
     }
 
     State.thrustFiring = 1;
@@ -269,6 +272,14 @@ void thrustInfo(Vehicle &State) {
 
   } else {
     State.thrust = 0.0;
+
+    State.Fx = 0.0;
+    State.Fy = 0.0;
+    State.Fz = g * State.massInitial;
+
+    State.momentX = 0.0;
+    State.momentY = 0.0;
+    State.momentZ = 0.0;
   }
 }
 
diff --git a/src/main.cpp b/src/main.cpp
index 268e056..84b0855 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -62,6 +62,7 @@ void setup() {
 
 void loop() {
   last = micros();
+  Serial.println(State.vz);
 
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
@@ -88,8 +89,13 @@ void loop() {
     teensyAbort();
   }
 
-  State.stepDuration = micros() - last;
-  delayMicroseconds((1000.0 * State.stepSize) - State.stepDuration);
+  if (State.burnElapsed < 0.01) {
+    State.stepDuration = State.stepSize;
+
+  } else {
+    State.stepDuration = micros() - last;
+    delayMicroseconds((1000.0 * State.stepSize) - State.stepDuration);
+  }
 }
 
 #endif