diff --git a/include/teensy.h b/include/teensy.h index 06fa3ae..62d7076 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -214,5 +214,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 d9cfb0a..21d20d5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,6 +11,7 @@ #elif defined(TEENSY) #include +int BUILTIN_LED = 13; unsigned long last; #endif @@ -90,16 +91,14 @@ void loop() { if ((State.z < 0.0) && (State.thrustFiring == 2)) { 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(20000); - Serial.println("Restarting Sim"); - Serial.println( - "==============================================================="); + Serial.println("SUCCESS"); + Serial.println("Aborting Sim"); + teensyAbort(); } } #endif