From 7cffb33ff19c6f5fc8b5e049d1f7f8aa87e90656 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Mon, 8 Nov 2021 13:54:12 -0700
Subject: [PATCH] donezo

---
 include/teensy.h |  4 ++++
 src/main.cpp     | 14 +++++++-------
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index 283e721..027b542 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -232,5 +232,9 @@ void closeFile() {
 
 void teensyAbort() {
   while (1) {
+    digitalWrite(BUILTIN_LED, HIGH);
+    delay(1000);
+    digitalWrite(BUILTIN_LED, LOW);
+    delay(1000);
   }
 }
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index d1a400e..71d76d0 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -11,6 +11,7 @@
 #elif defined(TEENSY)
 #include <Arduino.h>
 
+int BUILTIN_LED = 13;
 unsigned long last;
 #endif
 
@@ -36,6 +37,8 @@ void setup() {
 }
 #elif defined(TEENSY)
 void setup() {
+  pinMode(BUILTIN_LED, OUTPUT);
+  digitalWrite(BUILTIN_LED, HIGH);
   delay(1000);
   init_Vehicle(State);
   Serial.println("Simulated Vehicle Initalized");
@@ -91,16 +94,13 @@ void loop() {
 
   if (State.z < 0.0) {
     printSimResults(State);
-    init_Vehicle(State);
-    delay(1000);
-    Serial.println("Last run duration:" + String(millis() - last) + " ms");
+    Serial.println("Run duration:" + String(millis() - last) + " ms");
 
     closeFile();
-    delay(10000);
 
-    Serial.println("Restarting Sim");
-    Serial.println(
-        "===============================================================");
+    Serial.println("SUCCESS");
+    Serial.println("Aborting Sim");
+    teensyAbort();
   }
 }
 #endif