added else statement

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

@@ -1,10 +1,11 @@
-#include <iostream>
-#include <thread>
-#include <fstream>
-#include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
-#include <opencv4/opencv2/opencv.hpp>
+#include "MQTT/MqttClient.h"
+#include <fstream>
+#include <iostream>
 #include <opencv4/opencv2/core.hpp>
+#include <opencv4/opencv2/opencv.hpp>
+#include <thread>
+
 
 using namespace std;
 using namespace cv;
@@ -16,13 +17,13 @@ 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
+MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi",
+                  "rpiwachtwoordofzo"); // create a client object
 std::string message = "stop";
 std::string serializeKobukiData(const TKobukiData &data);
 void sendKobukiData(TKobukiData &data);
 
-void setup()
-{
+void setup() {
   unsigned char *null_ptr(0);
   robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
   // connect mqtt server and sub to commands
@@ -30,21 +31,17 @@ void setup()
   client.subscribe("home/commands");
 }
 
-int main()
-{
+int main() {
   setup();
   std::thread image(CapnSend);
-  std::thread safety([&](){ robot.robotSafety(&message); });
-  std::thread sendMqtt([&](){ sendKobukiData(robot.parser.data); });
+  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)
-  {
+  while (true) {
     std::string message = readMQTT();
-    if (!message.empty())
-    {
+    if (!message.empty()) {
       parseMQTT(message);
     }
   }
@@ -56,16 +53,12 @@ int main()
 
 std::mutex connectionMutex;
 
-void checkKobukiConnection()
-{
-    while (true)
-    {
+void checkKobukiConnection() {
+  while (true) {
     {
       std::lock_guard<std::mutex> lock(connectionMutex);
-            if (!robot.isConnected())
-            {
-                if (kobuki_connected)
-                {
+      if (!robot.isConnected()) {
+        if (kobuki_connected) {
           cout << "Kobuki is disconnected" << endl;
           kobuki_connected = false;
         }
@@ -73,29 +66,29 @@ void checkKobukiConnection()
         cout << "Attempting to reconnect Kobuki..." << endl;
         robot.startCommunication("/dev/ttyUSB0", true, nullptr);
         kobuki_connected = robot.isConnected();
-                if (kobuki_connected)
-                {
+        if (kobuki_connected) {
           cout << "Kobuki reconnected successfully!" << endl;
+        } else {
+          cout << "Failed to reconnect Kobuki, retrying in 5 seconds..."
+               << 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::this_thread::sleep_for(
+          std::chrono::seconds(5)); // Controleer elke 5 seconden
     }
-        std::this_thread::sleep_for(std::chrono::seconds(5)); // Controleer elke 5 seconden
   }
 }
 
-
-
-std::string readMQTT()
-{
+std::string readMQTT() {
   static std::string lastMessage;
 
   std::string message = client.getLastMessage();
-  if (!message.empty() && message != lastMessage)
-  {
+  if (!message.empty() && message != lastMessage) {
     std::cout << "MQTT Message: " << message << std::endl;
     lastMessage = message;
   }
@@ -105,49 +98,32 @@ std::string readMQTT()
   return lastMessage;
 }
 
-void parseMQTT(std::string message)
-{
-  if (message == "up")
-  {
+void parseMQTT(std::string message) {
+  if (message == "up") {
     robot.forward(350);
-  }
-  else if (message == "left")
-  {
+  } else if (message == "left") {
     robot.setRotationSpeed(4);
-  }
-  else if (message == "right")
-  {
+  } else if (message == "right") {
     robot.setRotationSpeed(-4);
-  }
-  else if (message == "down")
-  {
+  } else if (message == "down") {
     robot.forward(-350);
-  }
-  else if (message == "stop")
-  {
+  } else if (message == "stop") {
     robot.sendNullMessage();
     robot.sendNullMessage();
-  }
-  else if (message == "estop")
-  {
+  } else if (message == "estop") {
     robot.forward(-400);
-  }
-  else
-  {
+  } else {
     std::cout << "Invalid command" << std::endl;
   }
 }
 
-void logToFile()
-{
-  while (true)
-  {
+void logToFile() {
+  while (true) {
     TKobukiData robotData = robot.parser.data;
     std::ofstream outputFile("log",
                              std::ios_base::app); // Open file in append mode to
                                                   // not overwrite own content
-    if (outputFile.is_open())
-    { // check if the file was opened successfully
+    if (outputFile.is_open()) { // check if the file was opened successfully
       // Get current time
       std::time_t now = std::time(nullptr);
       outputFile << "Timestamp: " << std::ctime(&now);
@@ -205,9 +181,7 @@ void logToFile()
       outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n";
       outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n";
       outputFile.close();
-    }
-    else
-    {
+    } else {
       std::cerr << "Error opening file\n";
     }
 
@@ -215,10 +189,8 @@ void logToFile()
   }
 }
 
-void sendIndividualKobukiData(const TKobukiData &data)
-{
-  while (true)
-  {
+void sendIndividualKobukiData(const TKobukiData &data) {
+  while (true) {
     std::cout << "Kobuki Data wordt gepubliceerd naar kobuki/data/timestamp: "
               << data.timestamp << std::endl;
     client.publishMessage("kobuki/data/timestamp",
@@ -306,8 +278,7 @@ void sendIndividualKobukiData(const TKobukiData &data)
     client.publishMessage("kobuki/data/extraInfo/UDID2",
                           std::to_string(data.extraInfo.UDID2));
 
-    if (!data.gyroData.empty())
-    {
+    if (!data.gyroData.empty()) {
       const auto &latestGyro = data.gyroData.back();
       client.publishMessage("kobuki/data/gyroData/x",
                             std::to_string(latestGyro.x));
@@ -321,8 +292,7 @@ void sendIndividualKobukiData(const TKobukiData &data)
   }
 }
 
-std::string serializeKobukiData(const TKobukiData &data)
-{
+std::string serializeKobukiData(const TKobukiData &data) {
   std::string json =
       "{\"timestamp\":" + std::to_string(data.timestamp) +
       ",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
@@ -375,8 +345,7 @@ std::string serializeKobukiData(const TKobukiData &data)
       ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
       ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
 
-  if (!data.gyroData.empty())
-  {
+  if (!data.gyroData.empty()) {
     const auto &latestGyro = data.gyroData.back();
     json += "{\"x\":" + std::to_string(latestGyro.x) +
             ",\"y\":" + std::to_string(latestGyro.y) +
@@ -388,10 +357,8 @@ std::string serializeKobukiData(const TKobukiData &data)
 }
 // create extra function to send the message every 100ms
 // needed it so it can be threaded
-void sendKobukiData(TKobukiData &data)
-{
-  while (true)
-  {
+void sendKobukiData(TKobukiData &data) {
+  while (true) {
     client.publishMessage("kobuki/data", serializeKobukiData(data));
     std::cout << "Sent data" << std::endl;
     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
@@ -415,7 +382,8 @@ void CapnSend() {
       cap.open(0);
       if (!cap.isOpened()) {
         cerr << "Error: Could not reconnect to camera" << endl;
-                std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying
+        std::this_thread::sleep_for(
+            std::chrono::seconds(1)); // Wait before retrying
         continue;
       } else {
         cout << "Reconnected to camera" << endl;
@@ -432,6 +400,7 @@ void CapnSend() {
     client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
     cout << "Sent image" << endl;
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Send image every 200ms
+    std::this_thread::sleep_for(
+        std::chrono::milliseconds(200)); // Send image every 200ms
   }
 }