From b75069986f25c1f4699172dcbe8a682ba322a3a9 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Mon, 29 Nov 2021 11:03:17 -0700
Subject: [PATCH] New sim exit condition and fixed servo gimbal test

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

diff --git a/include/teensy.h b/include/teensy.h
index 9659c10..94d76e0 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -45,7 +45,7 @@ const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
 void testGimbal(PWMServo &yaw, PWMServo &pitch) {
-  int servoTest = 90;
+  int servoTest = 0;
 
   if ((!yaw.attached()) || (!pitch.attached())) {
     Serial.println("Servo pin assignments are not valid!");
@@ -56,6 +56,7 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   pitch.write(pitch_conv(servoTest));
 
   // Servo 1 Test
+  Serial.println("1");
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
     yaw.write(yaw_conv(servoTest));
     delay(30);
@@ -68,6 +69,7 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   delay(1000);
 
   // Servo 2 Test
+  Serial.println("2");
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
     pitch.write(pitch_conv(servoTest));
     delay(30);
@@ -80,6 +82,7 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   delay(1000);
 
   // Servo 1 & 2 Test
+  Serial.println("1 and 2");
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
     yaw.write(yaw_conv(servoTest));
     pitch.write(pitch_conv(servoTest));
@@ -93,8 +96,8 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
 
   delay(30);
 
-  yaw.write(yaw_conv(90));
-  pitch.write(pitch_conv(90));
+  yaw.write(yaw_conv(0));
+  pitch.write(pitch_conv(0));
 }
 
 void initLoadCells(Vehicle &State) {
@@ -229,14 +232,11 @@ void thrustInfo(Vehicle &State) {
 
     digitalWrite(pin_igniter, HIGH);
     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);
-
     // Constants based on vehicle
     double r = 3.0;
     double R = 5.0;
@@ -248,6 +248,12 @@ void thrustInfo(Vehicle &State) {
     State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
     State.thrust = sqrt(pow(State.Fz, 2) + pow(State.Fx, 2) + pow(State.Fy, 2));
 
+    if (State.burnElapsed > 4.0) {
+      State.thrustFiring = 2;
+      digitalWrite(pin_igniter, LOW);
+      State.thrust = 0.0;
+    }
+
   } else {
     State.thrust = 0.0;
   }
diff --git a/src/main.cpp b/src/main.cpp
index d5814f9..a11dbac 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -75,8 +75,7 @@ void loop() {
   PrevState = State;
   State.time += State.stepSize;
 
-  if ((State.z < 0.0)) {
-    digitalWrite(pin_igniter, LOW);
+  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     Serial.println("Run duration:" + String((micros() - initTime) / 1e6) +
                    " seconds");
     printSimResults(State);