From 2c630bf89ba53814bd439587e78b25b488248d06 Mon Sep 17 00:00:00 2001
From: "ishak jmilou.ishak" <ishak.jmiloui@hva.nl>
Date: Mon, 20 Jan 2025 12:11:38 +0100
Subject: [PATCH] refactor: improve Kobuki connection handling and add USB
 device check

---
 src/C++/Driver/src/main.cpp | 67 ++++++++++++++++++++-----------------
 1 file changed, 36 insertions(+), 31 deletions(-)

diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index e83c870..c3572ee 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -60,40 +60,45 @@ void checkKobukiConnection()
 {
     while (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;
-                }
-            }else{
-                if (!kobuki_connected)
-                {
-                    cout << "Kobuki is connected" << endl;
-                    kobuki_connected = true;
-                }
+        std::lock_guard<std::mutex> lock(connectionMutex);
+
+        // Controleer of het apparaat beschikbaar is
+        if (!std::ifstream("/dev/ttyUSB0")){
+            if (kobuki_connected){
+                cout << "Kobuki disconnected: USB device not found." << endl;
+                kobuki_connected = false;
+            }
+            std::this_thread::sleep_for(std::chrono::seconds(5));
+            continue; // Probeer later opnieuw
         }
-        std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+
+        // Controleer of de Kobuki verbonden is
+        if (!robot.isConnected()){
+            if (kobuki_connected){
+                cout << "Kobuki disconnected." << endl;
+                kobuki_connected = false;
+            }
+
+            cout << "Attempting to reconnect Kobuki..." << endl;
+            robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+
+            if (robot.isConnected()){
+                cout << "Kobuki reconnected successfully!" << endl;
+                kobuki_connected = true;
+            }
+            else{
+                cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
+            }
+        }else if (!kobuki_connected){
+            // Update status als de verbinding hersteld is
+            cout << "Kobuki is connected." << endl;
+            kobuki_connected = true;
+        }
+
+        // Wacht voordat je opnieuw controleert
+        std::this_thread::sleep_for(std::chrono::seconds(5));
     }
 }
-}
-
-
 
 std::string readMQTT()
 {