From aedff1c2cc73ed88dabbe018faf85d5854accf2b Mon Sep 17 00:00:00 2001
From: "ishak jmilou.ishak" <ishak.jmiloui@hva.nl>
Date: Mon, 20 Jan 2025 10:58:34 +0100
Subject: [PATCH] made more debug

---
 src/C++/Driver/src/main.cpp | 33 ++++++++++++++++++++++-----------
 1 file changed, 22 insertions(+), 11 deletions(-)

diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 494085f..8420633 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -60,24 +60,35 @@ 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::lock_guard<std::mutex> lock(connectionMutex);
+            if (!robot.isConnected())
+            {
+                if (kobuki_connected)
+                {
+                    cout << "Kobuki is disconnected" << endl;
+                    kobuki_connected = false;
+                }
+                // Probeer opnieuw te verbinden
+                cout << "Attempting to reconnect Kobuki..." << endl;
+                robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+                kobuki_connected = robot.isConnected();
+                if (kobuki_connected)
+                {
+                    cout << "Kobuki reconnected successfully!" << endl;
+                }
+                else
+                {
+                    cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
+                }
+            }
         }
         std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
     }
 }
 
 
+
 std::string readMQTT()
 {
   static std::string lastMessage;