From 053d2b972654782903bcf2658425065d62cf0b62 Mon Sep 17 00:00:00 2001
From: Thefeli73 <thefeli73@hotmail.com>
Date: Wed, 14 Apr 2021 01:54:01 +0200
Subject: [PATCH] adding conditions to run things ever x ms

---
 EENX15_LQR/EENX15_LQR.ino | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/EENX15_LQR/EENX15_LQR.ino b/EENX15_LQR/EENX15_LQR.ino
index 6fd8020..7efe2aa 100644
--- a/EENX15_LQR/EENX15_LQR.ino
+++ b/EENX15_LQR/EENX15_LQR.ino
@@ -1,5 +1,9 @@
 #include <Wire.h>
 
+
+int lastCorrectionTime = 0;
+int lastPrintTime = 0;
+
 //temporary variable to measure main loops
 int temp_loops;
 
@@ -100,7 +104,18 @@ void loop() {
   gyro_loop();
   safe_angle = int(round(angle_pitch_output));
 
-  if(temp_loops>250){
+  int m = millis();
+  
+  if (m - lastCorrectionTime >= 20) { //run this code ever 20ms (50hz)
+    lastCorrectionTime = m;
+    getSpeed();
+    setSpeed();
+  }
+  if (m - lastPrintTime >= 800) { //run this code ever 800ms (1.25hz)
+    lastPrintTime = m;
+    printInfo();
+  }
+}
     Serial.println("");
     Serial.print(" pitch Angle  = "); Serial.println(angle_pitch_output);
     Serial.print(" pitch Angle abs = "); Serial.println(abs(safe_angle));