new reconnect function

src/C++/Driver/src/main.cpp

@@ -1,5 +1,6 @@
 #include <iostream>
 #include <thread>
+#include <fstream>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
 #include <opencv4/opencv2/opencv.hpp>
@@ -13,6 +14,7 @@ std::atomic<bool> kobuki_connected(false);
 std::string readMQTT();
 void parseMQTT(std::string message);
 void CapnSend();
+void checkKobukiConnection();
 // ip, clientID, username, password
 MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
 std::string message = "stop";
@@ -24,37 +26,19 @@ void setup()
   unsigned char *null_ptr(0);
   robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
   // connect mqtt server and sub to commands
-
   client.connect();
   client.subscribe("home/commands");
 }
 
-void checkKobukiConnection()
-{
-  while (true)
-  {
-    bool connected = robot.isConnected();
-    if (!connected && kobuki_connected)
-    {
-      cout << "Kobuki is disconnected" << endl;
-      kobuki_connected = false;
-    }
-    else if (connected && !kobuki_connected)
-    {
-      cout << "Kobuki is connecting..." << endl;
-      // Start de Kobuki automatisch
-      robot.startCommunication("/dev/ttyUSB0", true, nullptr);
-    }
-    std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
-  }
-}
-
 int main()
 {
   setup();
   std::thread image(CapnSend);
   std::thread safety([&](){ robot.robotSafety(&message); });
   std::thread sendMqtt([&](){ sendKobukiData(robot.parser.data); });
+  std::thread connectionChecker(checkKobukiConnection);
+  connectionChecker.detach(); // Laat deze thread onafhankelijk draaien
+ 
 
   while (true)
   {
@@ -70,6 +54,30 @@ int main()
   image.join();
 }
 
+std::mutex connectionMutex;
+
+void checkKobukiConnection()
+{
+    while (true)
+    {
+        std::lock_guard<std::mutex> lock(connectionMutex);
+        bool connected = robot.isConnected();
+        if (!connected && kobuki_connected)
+        {
+            cout << "Kobuki is disconnected" << endl;
+            kobuki_connected = false;
+        }
+        else if (connected && !kobuki_connected)
+        {
+            cout << "Kobuki is reconnecting..." << endl;
+            robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+            kobuki_connected = true;
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+    }
+}
+
+
 std::string readMQTT()
 {
   static std::string lastMessage;