refactor: improve Kobuki connection handling and add USB device check

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

@@ -61,23 +61,45 @@ 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;
+
+        // 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
         }
-        else if (connected && !kobuki_connected)
-        {
-            cout << "Kobuki is reconnecting..." << endl;
+
+        // 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;
         }
-        std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+
+        // Wacht voordat je opnieuw controleert
+        std::this_thread::sleep_for(std::chrono::seconds(5));
     }
 }
 
-
 std::string readMQTT()
 {
   static std::string lastMessage;