diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 3a5aef4..589476d 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -8,6 +8,8 @@
 using namespace std;
 using namespace cv;
 CKobuki robot;
+std::atomic<bool> kobuki_connected(false);
+
 
 std::string readMQTT();
 void parseMQTT(std::string message);
@@ -28,6 +30,24 @@ void setup()
     client.subscribe("home/commands");
 }
 
+void checkKobukiConnection() {
+    while (true) {
+        bool connected = robot.isConnected();
+        if (connected && !kobuki_connected) {
+            cout << "Kobuki is connected" << endl;
+            kobuki_connected = true;
+            // Start de Kobuki automatisch
+            robot.startCommunication("/dev/ttyUSB0", true, nullptr);
+        } else if (!connected && kobuki_connected) {
+            cout << "Kobuki is disconnected" << endl;
+            kobuki_connected = false;
+            // Stop de Kobuki automatisch
+            robot.sendNullMessage();
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+    }
+}
+
 int main()
 {
     setup();
@@ -46,6 +66,7 @@ int main()
     sendMqtt.join();
     safety.join();
     image.join();
+    connectionThread.join();
 }
 
 std::string readMQTT()