added debug class for time measurment

src/main.cpp

@@ -15,6 +15,7 @@
 #include "moveControl.h"
 #include "route.h"
 #include "debugMqtt.h"
+#include "debugTimes.h"
 
 void callbackControllerAction();
 void callbackControllerConnect();
@@ -22,22 +23,6 @@ void controllerPrintBattery();
 
 bool reconnectMqtt();
 
-// TODO: This is a temporary function for testing
-void checkTimes(bool set, uint16_t maxTime,const char *name) {
-    static uint64_t lastMillis = 0;
-    if (set) {
-        lastMillis = millis();
-    } else {
-        uint64_t pastTime = millis() - lastMillis;
-        if (pastTime > maxTime) {
-            Serial.print("Function: ");
-            Serial.print(name);
-            Serial.print("-Time: ");
-            Serial.println(pastTime);
-        }
-    }
-}
-
 MotorControl left_motor;
 MotorControl right_motor;
 Speedometer speedometer_left;
@@ -90,6 +75,7 @@ void setup() {
 
     // mqtt_client.setServer(mqtt_server, MQTT_PORT);
     mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
+    mqtt_client.setSocketTimeout(1);
     reconnectMqtt();
 
     //FIXME: make it from config.h
@@ -120,15 +106,14 @@ void setup() {
     consolControl.init(&moveController);
 
     //Test checkTimes function
-    checkTimes(true, 0, "");
+    DebugTimes testDebugTimes;
     delay(2);
-    checkTimes(false, 1, "Test checkTimes");
-
-    //Mqtt set down Sockettimeout
-    mqtt_client.setSocketTimeout(1);
+    testDebugTimes.stop("Test checkTimes");
 }
 
 void loop() {
+
+
     if (!mqtt_client.connected()) {
     long now = millis();
     if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) {
@@ -140,31 +125,30 @@ void loop() {
     }
   } else {
     // Client connected
-    checkTimes(true, 0, "");
+    DebugTimes mqttClientLoop;
     mqtt_client.loop();
-    checkTimes(false, 100, "mqttloop");
+    mqttClientLoop.stop("mqttloop", 100);
   }   
 
-    checkTimes(true, 0, "");
+    DebugTimes runMotorLeft;
     left_motor.runMotorControl();
-    checkTimes(false, 100, "left_motor");
-    checkTimes(true, 0, "");
+    runMotorLeft.stop("left_motor", 100);
+
+    DebugTimes runMotorRight;
     right_motor.runMotorControl();
-    checkTimes(false, 100, "right_motor");
-    checkTimes(true, 0, "");
+    runMotorRight.stop("right_motor", 100);
+    
     speedometer_left.runSpeedometer();
-    checkTimes(false, 100, "left_speed");
-    checkTimes(true, 0, "");
     speedometer_right.runSpeedometer();
-    checkTimes(false, 100, "right_speed");
     // TODO: Auskommentiert weil macht vielleicht komische Sachen
     // route.runRoute();
 
     switch (driveMode) {
-        case manualControl_e:
-            checkTimes(true, 0, "");
+        case manualControl_e: {
+            DebugTimes manControlDebug;
             manualControl.runManualControl();
-            checkTimes(false, 100, "manualControl run");
+            manControlDebug.stop("manualControl run", 100);
+            }
             break;
 
         case captureRoute_e: