improve pid and other stuff

src/main.cpp

@@ -16,13 +16,27 @@
 #include "route.h"
 #include "debugMqtt.h"
 
-//TODO: Mal vernünftig Programmieren lernen.
-
 void callbackControllerAction();
 void callbackControllerConnect();
 void controllerPrintBattery();
 
-void reconnectMqtt();
+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;
@@ -45,6 +59,7 @@ IPAddress subnet(WLAN_SUBNETMASK);
 WiFiClient wifi_client;
 PubSubClient mqtt_client(wifi_client);
 
+uint64_t lastReconnectAttempt = 0;
 int controller_battery = -1;
 enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
 DriveMode driveMode = manualControl_e;
@@ -77,7 +92,8 @@ void setup() {
     mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
     reconnectMqtt();
 
-    configTime(3600, 3600, "192.168.1.2");
+    //FIXME: make it from config.h
+    configTime(3600, 3600, "2.de.pool.ntp.org");
 
     DebugMqtt::init(&mqtt_client, Loglevel::debug);
     DebugMqtt::initRealMillis();
@@ -101,25 +117,54 @@ void setup() {
     manualControl.init(&moveController, &left_motor, &right_motor);
     captureRoute.init(&route, &moveController, &left_motor, &right_motor);
     autopilot.init(&route, &moveController);
-    Serial.println("main 1");
     consolControl.init(&moveController);
-    Serial.println("main 2");
+
+    //Test checkTimes function
+    checkTimes(true, 0, "");
+    delay(2);
+    checkTimes(false, 1, "Test checkTimes");
+
+    //Mqtt set down Sockettimeout
+    mqtt_client.setSocketTimeout(1);
 }
 
 void loop() {
-    if (!mqtt_client.connected())
-        reconnectMqtt();
+    if (!mqtt_client.connected()) {
+    long now = millis();
+    if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) {
+      lastReconnectAttempt = now;
+      // Attempt to reconnect
+      if (reconnectMqtt()) {
+        lastReconnectAttempt = 0;
+      }
+    }
+  } else {
+    // Client connected
+    checkTimes(true, 0, "");
     mqtt_client.loop();
+    checkTimes(false, 100, "mqttloop");
+  }   
 
+    checkTimes(true, 0, "");
     left_motor.runMotorControl();
+    checkTimes(false, 100, "left_motor");
+    checkTimes(true, 0, "");
     right_motor.runMotorControl();
+    checkTimes(false, 100, "right_motor");
+    checkTimes(true, 0, "");
     speedometer_left.runSpeedometer();
+    checkTimes(false, 100, "left_speed");
+    checkTimes(true, 0, "");
     speedometer_right.runSpeedometer();
-    route.runRoute();
+    checkTimes(false, 100, "right_speed");
+    // TODO: Auskommentiert weil macht vielleicht komische Sachen
+    // route.runRoute();
 
     switch (driveMode) {
         case manualControl_e:
+            checkTimes(true, 0, "");
             manualControl.runManualControl();
+            checkTimes(false, 100, "manualControl run");
             break;
 
         case captureRoute_e:
@@ -139,37 +184,26 @@ void loop() {
     }
 }
 
-void reconnectMqtt() {
-    // Loop until reconnection
-    while (!mqtt_client.connected()) {
-        Serial.print("Attempting MQTT connection...");
-        // Create a random client ID
-        String clientId = "ESP32Rover-";
-        clientId += String(random(0xffff), HEX);
-        // Attempt to connect
-        #ifdef MQTT_AUTH
-        if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) {
-        #endif //MQTT_AUTH
-        #ifndef MQTT_AUTH
-        if (mqtt_client.connect(clientId.c_str())) {
-        #endif //MQTT_AUTH
-            Serial.println("connected");
-            // Once connected, publish an announcement...
-            mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
-        } else {
-            Serial.print("failed, rc=");
-            Serial.print(mqtt_client.state());
-            Serial.println(" try again in 5 seconds");
-            // Wait 5 seconds before retrying
-            delay(5000);
-        }
+bool reconnectMqtt() {
+    // Create a random client ID
+    String clientId = "ESP32Rover-";
+    clientId += String(random(0xffff), HEX);
+    #ifdef MQTT_AUTH
+    if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) {
+    #endif //MQTT_AUTH
+    #ifndef MQTT_AUTH
+    if (mqtt_client.connect(clientId.c_str())) {
+    #endif //MQTT_AUTH
+        // Once connected, publish an announcement...
+        mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
     }
+    return mqtt_client.connected();
 }
 
 void callbackControllerAction() {
     if (Ps3.event.button_down.ps) {
         switch (driveMode) {
-            case manualControl_e:
+        case manualControl_e:
             driveMode = DriveMode::captureRoute_e;
             debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
             break;