laat kobuki nu connecten op open ports

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

@@ -1,6 +1,7 @@
 #include <iostream>
 #include <thread>
 #include <fstream>
+#include <filesystem>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
 #include <opencv4/opencv2/opencv.hpp>
@@ -21,6 +22,19 @@ std::string message = "stop";
 std::string serializeKobukiData(const TKobukiData &data);
 void sendKobukiData(TKobukiData &data);
 
+std::string findKobukiPort()
+{
+    for (const auto& entry : std::filesystem::directory_iterator("/dev"))
+    {
+        std::string device = entry.path().string();
+        if (device.find("ttyUSB") != std::string::npos)
+        {
+            return device; // Returneer de eerste gevonden poort
+        }
+    }
+    return ""; // Geen poort gevonden
+}
+
 void setup()
 {
   unsigned char *null_ptr(0);
@@ -61,6 +75,7 @@ void checkKobukiConnection()
     while (true)
     {
         std::lock_guard<std::mutex> lock(connectionMutex);
+        std::string port = findKobukiPort();
 
         // Controleer of het apparaat beschikbaar is
         if (!std::ifstream("/dev/ttyUSB0")){
@@ -80,7 +95,7 @@ void checkKobukiConnection()
             }
 
             cout << "Attempting to reconnect Kobuki..." << endl;
-            robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+            robot.startCommunication(port.c_str(), true, nullptr);
 
             if (robot.isConnected()){
                 cout << "Kobuki reconnected successfully!" << endl;
@@ -89,12 +104,7 @@ void checkKobukiConnection()
             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));
     }