Print Timing Info

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"
@@ -75,6 +75,8 @@ void setup() {
   delay(1000);
   initFile();
   delay(1000);
+
+  initTime = micros();
 }
 #endif
 
@@ -100,19 +102,28 @@ void loop() {
 #elif defined(TEENSY)
 void loop() {
 
-  last = millis();
+  last = micros();
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
+  
   processTVC(State, loadCells);
+
+  State.stepDuration = micros() - last;
+  write2CSV(State);
+
+  // Set "prev" values for next timestep
+  PrevState = State;
+  
   // state2vec(State, PrevState, stateVector);
 
+
   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);
@@ -121,6 +132,8 @@ void loop() {
     Serial.println("Aborting Sim");
     teensyAbort();
   }
+
+  delay(20 - ((micros() - last) * 1000.0));
 }
 #endif