From 1ab718a4720e909476363bdcc36da9332d18eaad Mon Sep 17 00:00:00 2001
From: "ishak jmilou.ishak" <ishak.jmiloui@hva.nl>
Date: Thu, 16 Jan 2025 13:55:24 +0100
Subject: [PATCH] new reconnect function

---
 src/C++/Driver/src/main.cpp | 50 +++++++++++++++++++++----------------
 1 file changed, 29 insertions(+), 21 deletions(-)

diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 5fb1c69..1b8997e 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -1,5 +1,6 @@
 #include <iostream>
 #include <thread>
+#include <fstream>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
 #include <opencv4/opencv2/opencv.hpp>
@@ -13,6 +14,7 @@ std::atomic<bool> kobuki_connected(false);
 std::string readMQTT();
 void parseMQTT(std::string message);
 void CapnSend();
+void checkKobukiConnection();
 // ip, clientID, username, password
 MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
 std::string message = "stop";
@@ -24,37 +26,19 @@ void setup()
   unsigned char *null_ptr(0);
   robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
   // connect mqtt server and sub to commands
-
   client.connect();
   client.subscribe("home/commands");
 }
 
-void checkKobukiConnection()
-{
-  while (true)
-  {
-    bool connected = robot.isConnected();
-    if (!connected && kobuki_connected)
-    {
-      cout << "Kobuki is disconnected" << endl;
-      kobuki_connected = false;
-    }
-    else if (connected && !kobuki_connected)
-    {
-      cout << "Kobuki is connecting..." << endl;
-      // Start de Kobuki automatisch
-      robot.startCommunication("/dev/ttyUSB0", true, nullptr);
-    }
-    std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
-  }
-}
-
 int main()
 {
   setup();
   std::thread image(CapnSend);
   std::thread safety([&](){ robot.robotSafety(&message); });
   std::thread sendMqtt([&](){ sendKobukiData(robot.parser.data); });
+  std::thread connectionChecker(checkKobukiConnection);
+  connectionChecker.detach(); // Laat deze thread onafhankelijk draaien
+ 
 
   while (true)
   {
@@ -70,6 +54,30 @@ int main()
   image.join();
 }
 
+std::mutex connectionMutex;
+
+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::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
+    }
+}
+
+
 std::string readMQTT()
 {
   static std::string lastMessage;