updated set up docs

opencv camera logic rewrite

See merge request technische-informatica-sm3/ti-projectten/rooziinuubii79!5

README.md

@@ -1,8 +1,70 @@
-# TI-project - Kobuki
+# TI-project - exploration robot Kobuki
 
 ## Description
-This project is a kobuki that drives around in dangerous areas and detects objects in its path. It uses a camera to detect objects. The kobuki is able to drive around in a room and detect objects.
+This project is a kobuki that drives around in dangerous areas and detects objects in its path. It uses a camera to detect objects. The purpose of this project is to explore dangerous areas without risking human lives. You are able to control the robot using controller on the website. 
 
 ## Photos
 ![Kobuki](/docs/assets/KobukiPhoto.jpg)
 
+## Installation
+
+### Requirements
+
+- Kobuki robot
+- Raspberry Pi (minimum 3B)
+- Camera
+- power supply for Raspberry Pi
+- laptop or computer
+
+### Steps
+
+1. **Install Python and Pip**
+   - Ensure you have Python installed on your system. You can download it from [python.org](https://www.python.org/).
+   - Pip is the package installer for Python. It usually comes with Python, but you can install it separately if needed.
+
+2. **Clone Our Repository**
+    - Clone our repository to your local machine doing the following :
+        - Open your terminal
+        - Change the current working directory to the location where you want the cloned directory.
+        - Type `git clone https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
+  
+3. **Install the required packages**
+    - Install the following packages on the server: "docker docker-buildx mosquitto nginx"
+    - Install the following packages on the Raspberry Pi: "g++ make cmake libopencv-dev libssl-dev", https://github.com/eclipse-paho/paho.mqtt.c, https://github.com/eclipse-paho/paho.mqtt.cpp 
+
+
+
+4. **Run the project**
+
+    #### Server side
+    - Run the following commands in the terminal to start the website:
+        - `cd src/Python/flask`
+        - `sudo docker buildx build -t flaskapp:latest .`
+        - `sudo docker run --network="host" --restart=always flaskapp:latest`
+    - Run the following commands in the terminal to start the MQTT broker:
+        - `cd src/config/server/`
+        - `mosquitto -c mosquitto.conf`
+    - Run the following commands in the terminal to start the Nginx server:
+        - `cd src/config/server/`
+        -  `cp nginx.conf /etc/nginx/nginx.conf`
+        -  `cp nginx-sites.conf /etc/nginx/sites-enable/nginx-sites.conf`
+
+
+    #### Raspberry Pi side
+    - Run the following commands to build and start the driver:
+        - `cd src/C++/Driver`
+        - `cmake ..`
+        - `make`
+        - `./kobuki_driver`
+    - Run the following commands to autostart the driver on startup of the Raspberry Pi:
+        - `cd src/config/rpi/`
+        - `cp kobukiDriver.service /etc/systemd/system/kobukiDriver.service`
+        - `systemctl enable kobukiDriver.service`
+        - `systemctl start kobukiDriver.service`
+
+## Extra notes
+Dont forget to change the IP address in the `src/C++/Driver/src/main.cpp` file to the IP address of the server.
+
+
+
+

docs/Infrastructure/uml.md

@@ -0,0 +1,51 @@
+```mermaid
+classDiagram
+    class CKobuki {
+        +enableCommands(bool commands)
+        +loop(void *user_data, TKobukiData &Kobuki_data)
+        +startCommunication(char *portname, bool CommandsEnabled, void *userDataL)
+        +measure()
+        +setLed(int led1, int led2)
+        +setTranslationSpeed(int mmpersec)
+        +setRotationSpeed(double radpersec)
+        +setArcSpeed(int mmpersec, int radius)
+        +setSound(int noteinHz, int duration)
+        +setPower(int value)
+        +goStraight(long double distance)
+        +forward(int speedvalue)
+        +doRotation(long double th)
+    }
+
+    class FlaskApp {
+        +on_message(client, message)
+        +get_db()
+        +close_db(error)
+        +index()
+        +control()
+        +move()
+    }
+
+    class MQTTClient {
+        +connect()
+        +subscribe(topic)
+        +getLastMessage()
+        +isConnected()
+    }
+
+    FlaskApp --> MQTTClient : uses
+    FlaskApp --> CKobuki : controls
+
+    class RPI {
+        +KobukiCommunication()
+        +ESP32Communication()
+        +Camera()
+    }
+
+    class ESP32 {
+        +TVOC()
+        +DHT11()
+    }
+
+    RPI --> MQTTClient : communicates
+    MQTTClient --> CKobuki : communicates
+    RPI --> ESP32 : communicates

docs/code/kobuki-reconnect.md

@@ -0,0 +1,78 @@
+# Kobuki automatische reconnect
+
+Mijn taak was om de kobuki automatisch te reconnecten als de verbinding verbroken werd met de pi. nu moet je telkens handmatig de pi laten connecten met de kobuki, dit is niet handig als iemand niet weet hoe dit moet. 
+
+De connectie word gemaakt met ttyUSB0. Dat is de eerste poort die wij kunnen gebruiken voor de kobuki. 
+
+```cpp
+  robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+```
+
+ik heb een functie gemaakt die kijkt of de pi nog is verbonden aan de kobuki. in de if statement kijkt hij elke 5 seconden of de ttyusb0 poort beschikbaar is. 
+
+```cpp
+void checkKobukiConnection()
+{
+    while (true)
+    {
+        std::lock_guard<std::mutex> lock(connectionMutex);
+
+        // 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
+        }
+```	
+
+Hier kijk ik dan of de kobuki is geconnect, zoniet dan moet ie weer connecten met ttyUSB0
+
+```cpp
+ // 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;
+            }
+        }
+```        
+
+Nu heb ik het probleem dat als ik de kabel eruit steek en terug in de kobuki stop dat de pi niet meer wil connecten. Dit komt omdat het systeem denkt dat de poort "ttyUSB0" nog steeds gebruikt word. Als ik de kabel dan terug stop word de poort "ttyUSB1" gebruikt, omdat ttyusb0 niet word vrijgegeven.
+
+```bash
+ishak@raspberrypi:~ $ dmesg | tail -n 20
+[10516.084132] usb 1-1.3: Product: iClebo Kobuki
+[10516.084144] usb 1-1.3: Manufacturer: Yujin Robot
+[10516.084155] usb 1-1.3: SerialNumber: kobuki_AI02MQMK
+[10516.091210] ftdi_sio 1-1.3:1.0: FTDI USB Serial Device converter detected
+[10516.091414] usb 1-1.3: Detected FT232R
+[10516.099169] usb 1-1.3: FTDI USB Serial Device converter now attached to ttyUSB1
+[10574.100491] usb 1-1.3: USB disconnect, device number 34
+[10574.101596] ftdi_sio ttyUSB1: FTDI USB Serial Device converter now disconnected from ttyUSB1
+[10574.101735] ftdi_sio 1-1.3:1.0: device disconnected
+[10579.697816] usb 1-1.3: new full-speed USB device number 35 using dwc_otg
+[10579.829776] usb 1-1.3: New USB device found, idVendor=0403, idProduct=6001, bcdDevice= 6.00
+[10579.829821] usb 1-1.3: New USB device strings: Mfr=1, Product=2, SerialNumber=3
+[10579.829836] usb 1-1.3: Product: iClebo Kobuki
+[10579.829848] usb 1-1.3: Manufacturer: Yujin Robot
+[10579.829860] usb 1-1.3: SerialNumber: kobuki_AI02MQMK
+[10579.840148] ftdi_sio 1-1.3:1.0: FTDI USB Serial Device converter detected
+[10579.840351] usb 1-1.3: Detected FT232R
+[10579.842208] usb 1-1.3: FTDI USB Serial Device converter now attached to ttyUSB1
+[10612.745819] hwmon hwmon1: Voltage normalised
+[10614.761829] hwmon hwmon1: Undervoltage detected!
+```

docs/scrum/retrospective/retro_sprint_4.md

@@ -0,0 +1 @@
+# retro sprint 4

src/C++/Driver/src/KobukiDriver/CKobuki.cpp

@@ -68,58 +68,47 @@ int CKobuki::connect(char *comportT) {
   HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
   if (HCom == -1) {
-    printf("Kobuki nepripojeny\n");
+    std::cerr <<"unable to connect. retry in 1 second" << std::endl;
     return HCom;
   } else {
-    set_interface_attribs2(HCom, B115200,
-                           0); // set speed to 115,200 bps, 8n1 (no parity)
+    set_interface_attribs2(HCom, B115200,0); // set speed to 115,200 bps, 8n1 (no parity)
     set_blocking2(HCom, 0);    // set no blocking
-    /*  struct termios settings;
-      tcgetattr(HCom, &settings);
-
-      cfsetospeed(&settings, B115200); // baud rate
-      settings.c_cflag &= ~PARENB; // no parity
-      settings.c_cflag &= ~CSTOPB; // 1 stop bit
-      settings.c_cflag &= ~CSIZE;
-      settings.c_cflag |= CS8 | CLOCAL; // 8 bits
-      settings.c_lflag &= ~ICANON; // canonical mode
-      settings.c_cc[VTIME]=1;
-      settings.c_oflag &= ~OPOST; // raw output
-
-      tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
     tcflush(HCom, TCOFLUSH);
 
-    printf("Kobuki pripojeny\n");
+    std::cout<<"Kobuki connected" << std::endl;
     return HCom;
   }
 }
 
+bool CKobuki::isConnected() {
+    return (HCom != -1 && tcflush(HCom, TCOFLUSH) == 0);
+}
+
 unsigned char *CKobuki::readKobukiMessage() {
   unsigned char buffer[1];
   ssize_t Pocet;
   buffer[0] = 0;
   unsigned char *null_buffer(0);
-  // citame kym nezachytime zaciatok spravy
+
+  // Read until the start of the message is detected
   do {
     Pocet = read(HCom, buffer, 1);
   } while (buffer[0] != 0xAA);
-  // mame zaciatok spravy (asi)
+
+  // We have the start of the message (possibly)
   if (Pocet == 1 && buffer[0] == 0xAA) {
-    // citame dalsi byte
+    // Read the next byte
     do {
-
       Pocet = read(HCom, buffer, 1);
+    } while (Pocet != 1); // On Linux: -1, on Windows: 0
 
-    } while (Pocet != 1); // na linuxe -1 na windowse 0
-
-    // a ak je to druhy byte hlavicky
+    // If it is the second byte of the header
     if (Pocet == 1 && buffer[0] == 0x55) {
-      // precitame dlzku
+      // Read the length
       Pocet = read(HCom, buffer, 1);
 
-      // ReadFile(hCom, buffer, 1, &Pocet, NULL);
       if (Pocet == 1) {
-        // mame dlzku.. nastavime vektor a precitame ho cely
+        // We have the length; initialize a buffer and read the entire message
         int readLenght = buffer[0];
         unsigned char *outputBuffer =
             (unsigned char *)calloc(readLenght + 4, sizeof(char));
@@ -134,7 +123,7 @@ unsigned char *CKobuki::readKobukiMessage() {
           pct = pct + (Pocet == -1 ? 0 : Pocet);
         } while (pct != (readLenght + 1));
 
-        // tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho
+        // Here we can check what data is received from the Kobuki's serial interface
         // for(int i=0;i<outputBuffer[0]+2;i++)
         // {
         // 	printf("%x ",outputBuffer[i]);
@@ -162,8 +151,8 @@ void CKobuki::setLed(int led1, int led2) {
   pocet = write(HCom, &message, 8);
 }
 
-// tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe
-// napajania aktivne musi byt aj tak v kazdej sprave...
+// this function doesn't have much meaning by itself, the payload about them being external
+// power supplies must be active in every message anyway...
 void CKobuki::setPower(int value) {
   if (value == 1) {
     unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF};
@@ -251,7 +240,7 @@ void CKobuki::setSound(int noteinHz, int duration) {
 
 void CKobuki::startCommunication(char *portname, bool CommandsEnabled,
                                  void *userDataL) {
-  connect(portname);
+  if(connect(portname) != -1){
   enableCommands(CommandsEnabled);
   userData = userDataL;
 
@@ -262,18 +251,19 @@ void CKobuki::startCommunication(char *portname, bool CommandsEnabled,
     std::cerr << "Error creating thread: " << pthread_result << std::endl;
   }
 }
+}
 
 int CKobuki::measure() {
   while (stopVlakno == 0) {
     unsigned char *message = readKobukiMessage();
     if (message == NULL) {
-      // printf("vratil null message\n");
-      continue;
+    // printf("returned null message\n");
+    continue;
     }
     int ok = parser.parseKobukiMessage(parser.data, message);
 
-    // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
-    // citat
+    // the maximum callback function can take 20 ms, if it takes longer, we won't be able to do it
+    // read
     if (ok == 0) {
       loop(userData, parser.data);
     }
@@ -353,7 +343,7 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
   totalLeft += dLeft;
   totalRight += dRight;
 
-  // ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo?
+// if the sum of the new and previous is greater than 65536 then it overflowed?
   directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1);
   directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1);
   dTimestamp = (Kobuki_data.timestamp < prevTimestamp
@@ -387,7 +377,7 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
 
 // tells the kobuki to go a few meters forward or backward, the sign decides
 // the function compensates for walking straight with the controller, internally
-// it uses setArcSpeed and uses encoder data as feedback
+// it uses setArcSpeed ​​and uses encoder data as feedback
 void CKobuki::goStraight(long double distance) {
   long double u_translation =
       0; // controlled magnitude, speed of the robot in motion

src/C++/Driver/src/KobukiDriver/CKobuki.h

@@ -79,6 +79,7 @@ public:
 	void robotSafety(); //overload
 	void sendNullMessage();
 	bool safetyActive = false;
+	bool isConnected();
 	KobukiParser parser;
 
 

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

@@ -1,6 +1,7 @@
 #include <iostream>
-#include <cmath>
 #include <thread>
+#include <fstream>
+#include <filesystem>
 #include "MQTT/MqttClient.h"
 #include "KobukiDriver/CKobuki.h"
 #include <opencv4/opencv2/opencv.hpp>
@@ -9,315 +10,458 @@
 using namespace std;
 using namespace cv;
 CKobuki robot;
+std::atomic<bool> kobuki_connected(false);
+
 std::string readMQTT();
 void parseMQTT(std::string message);
 void CapnSend();
-//ip, clientID, username, password
+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";
 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);
-    robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
-    //connect mqtt server and sub to commands
-
-    client.connect();
-    client.subscribe("home/commands");
+  std::string port = findKobukiPort();
+  unsigned char *null_ptr(0);
+  robot.startCommunication(const_cast<char*>(port.c_str()), true, null_ptr);
+  // connect mqtt server and sub to commands
+  client.connect();
+  client.subscribe("home/commands");
 }
 
 int main()
 {
-    setup();
-    std::thread image (CapnSend);
-    std::thread safety([&]() { robot.robotSafety(&message); });
-    std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
-    
-    while(true){
-        std::string message = readMQTT();
-        if (!message.empty()){
-            parseMQTT(message);
-        }
-        
-    }
+  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
+ 
 
-    sendMqtt.join();
-    safety.join();
-    image.join();
+  while (true)
+  {
+    std::string message = readMQTT();
+    if (!message.empty())
+    {
+      parseMQTT(message);
+    }
+  }
+
+  sendMqtt.join();
+  safety.join();
+  image.join();
+}
+
+std::mutex connectionMutex;
+
+void checkKobukiConnection()
+{
+    while (true)
+    {
+        std::lock_guard<std::mutex> lock(connectionMutex);
+        std::string port = findKobukiPort();
+
+        if (port.empty()) {
+            if (kobuki_connected) {
+                cout << "Kobuki disconnected: No USB device found." << endl;
+                kobuki_connected = false;
+            }
+            std::this_thread::sleep_for(std::chrono::seconds(5));
+            continue; // Probeer later opnieuw
+        }
+
+        // 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(const_cast<char*>(port.c_str()), true, nullptr);
+
+            if (robot.isConnected()){
+                cout << "Kobuki reconnected successfully!" << endl;
+                kobuki_connected = true;
+            }
+            else{
+                cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
+            }
+        }
+        // Wacht voordat je opnieuw controleert
+        std::this_thread::sleep_for(std::chrono::seconds(5));
+    }
 }
 
 std::string readMQTT()
 {
-    static std::string lastMessage;
+  static std::string lastMessage;
 
-    std::string message = client.getLastMessage();
-    if (!message.empty() && message != lastMessage)
-    {
-        std::cout << "MQTT Message: " << message << std::endl;
-        lastMessage = message;
-    }
+  std::string message = client.getLastMessage();
+  if (!message.empty() && message != lastMessage)
+  {
+    std::cout << "MQTT Message: " << message << std::endl;
+    lastMessage = message;
+  }
 
-    // Add a small delay to avoid busy-waiting
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    return message;
+  // Add a small delay to avoid busy-waiting
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  return lastMessage;
 }
 
 void parseMQTT(std::string message)
 {
-    if (message == "up")
-    {
-        robot.forward(350);
-    }
-    else if (message == "left")
-    {
-        robot.setRotationSpeed(4);
-    }
-    else if (message == "right")
-    {
-        robot.setRotationSpeed(-4);
-    }
-    else if (message == "down")
-    {
-        robot.forward(-350);
-    }
-    else if (message == "stop")
-    {
-        robot.sendNullMessage();
-        robot.sendNullMessage();
-    }
-    else if (message == "estop")
-    {
-        robot.forward(-400);
-    }
-    else
-    {
-        std::cout << "Invalid command" << std::endl;
-    }
+  if (message == "up")
+  {
+    robot.forward(350);
+  }
+  else if (message == "left")
+  {
+    robot.setRotationSpeed(4);
+  }
+  else if (message == "right")
+  {
+    robot.setRotationSpeed(-4);
+  }
+  else if (message == "down")
+  {
+    robot.forward(-350);
+  }
+  else if (message == "stop")
+  {
+    robot.sendNullMessage();
+    robot.sendNullMessage();
+  }
+  else if (message == "estop")
+  {
+    robot.forward(-400);
+  }
+  else
+  {
+    std::cout << "Invalid command" << std::endl;
+  }
 }
 
 void logToFile()
 {
-    while (true)
+  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
+      // Get current time
+      std::time_t now = std::time(nullptr);
+      outputFile << "Timestamp: " << std::ctime(&now);
+      // Write data to the file
+      outputFile << "analogInputCh0: " << robotData.analogInputCh0 << "\n";
+      outputFile << "analogInputCh1: " << robotData.analogInputCh1 << "\n";
+      outputFile << "analogInputCh2: " << robotData.analogInputCh2 << "\n";
+      outputFile << "analogInputCh3: " << robotData.analogInputCh3 << "\n";
+      outputFile << "digitalInput: " << robotData.digitalInput << "\n";
+      outputFile << "timestamp: " << robotData.timestamp << "\n";
+      outputFile << "BumperCenter: " << robotData.BumperCenter << "\n";
+      outputFile << "BumperLeft: " << robotData.BumperLeft << "\n";
+      outputFile << "BumperRight: " << robotData.BumperRight << "\n";
+      outputFile << "WheelDropLeft: " << robotData.WheelDropLeft << "\n";
+      outputFile << "WheelDropRight: " << robotData.WheelDropRight << "\n";
+      outputFile << "CliffCenter: " << robotData.CliffCenter << "\n";
+      outputFile << "CliffLeft: " << robotData.CliffLeft << "\n";
+      outputFile << "CliffRight: " << robotData.CliffRight << "\n";
+      outputFile << "EncoderLeft: " << robotData.EncoderLeft << "\n";
+      outputFile << "EncoderRight: " << robotData.EncoderRight << "\n";
+      outputFile << "PWMleft: " << robotData.PWMleft << "\n";
+      outputFile << "PWMright: " << robotData.PWMright << "\n";
+      outputFile << "ButtonPress: " << robotData.ButtonPress1 << "\n";
+      outputFile << "ButtonPress: " << robotData.ButtonPress2 << "\n";
+      outputFile << "ButtonPress: " << robotData.ButtonPress3 << "\n";
+      outputFile << "Charger: " << robotData.Charger << "\n";
+      outputFile << "Battery: " << robotData.Battery << "\n";
+      outputFile << "overCurrent: " << robotData.overCurrent << "\n";
+      outputFile << "IRSensorRight: " << robotData.IRSensorRight << "\n";
+      outputFile << "IRSensorCenter: " << robotData.IRSensorCenter << "\n";
+      outputFile << "IRSensorLeft: " << robotData.IRSensorLeft << "\n";
+      outputFile << "GyroAngle: " << robotData.GyroAngle << "\n";
+      outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n";
+      outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n";
+      outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter
+                 << "\n";
+      outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n";
+      outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n";
+      outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight
+                 << "\n";
+      outputFile << "frameId: " << robotData.frameId << "\n";
+      outputFile << "HardwareVersionPatch: "
+                 << robotData.extraInfo.HardwareVersionPatch << "\n";
+      outputFile << "HardwareVersionMinor: "
+                 << robotData.extraInfo.HardwareVersionMinor << "\n";
+      outputFile << "HardwareVersionMajor: "
+                 << robotData.extraInfo.HardwareVersionMajor << "\n";
+      outputFile << "FirmwareVersionPatch: "
+                 << robotData.extraInfo.FirmwareVersionPatch << "\n";
+      outputFile << "FirmwareVersionMinor: "
+                 << robotData.extraInfo.FirmwareVersionMinor << "\n";
+      outputFile << "FirmwareVersionMajor: "
+                 << robotData.extraInfo.FirmwareVersionMajor << "\n";
+      outputFile << "UDID0: " << robotData.extraInfo.UDID0 << "\n";
+      outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n";
+      outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n";
+      outputFile.close();
+    }
+    else
     {
-        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
-            // Get current time
-            std::time_t now = std::time(nullptr);
-            outputFile << "Timestamp: " << std::ctime(&now);
-            // Write data to the file
-            outputFile << "analogInputCh0: " << robotData.analogInputCh0 << "\n";
-            outputFile << "analogInputCh1: " << robotData.analogInputCh1 << "\n";
-            outputFile << "analogInputCh2: " << robotData.analogInputCh2 << "\n";
-            outputFile << "analogInputCh3: " << robotData.analogInputCh3 << "\n";
-            outputFile << "digitalInput: " << robotData.digitalInput << "\n";
-            outputFile << "timestamp: " << robotData.timestamp << "\n";
-            outputFile << "BumperCenter: " << robotData.BumperCenter << "\n";
-            outputFile << "BumperLeft: " << robotData.BumperLeft << "\n";
-            outputFile << "BumperRight: " << robotData.BumperRight << "\n";
-            outputFile << "WheelDropLeft: " << robotData.WheelDropLeft << "\n";
-            outputFile << "WheelDropRight: " << robotData.WheelDropRight << "\n";
-            outputFile << "CliffCenter: " << robotData.CliffCenter << "\n";
-            outputFile << "CliffLeft: " << robotData.CliffLeft << "\n";
-            outputFile << "CliffRight: " << robotData.CliffRight << "\n";
-            outputFile << "EncoderLeft: " << robotData.EncoderLeft << "\n";
-            outputFile << "EncoderRight: " << robotData.EncoderRight << "\n";
-            outputFile << "PWMleft: " << robotData.PWMleft << "\n";
-            outputFile << "PWMright: " << robotData.PWMright << "\n";
-            outputFile << "ButtonPress: " << robotData.ButtonPress1 << "\n";
-            outputFile << "ButtonPress: " << robotData.ButtonPress2 << "\n";
-            outputFile << "ButtonPress: " << robotData.ButtonPress3 << "\n";
-            outputFile << "Charger: " << robotData.Charger << "\n";
-            outputFile << "Battery: " << robotData.Battery << "\n";
-            outputFile << "overCurrent: " << robotData.overCurrent << "\n";
-            outputFile << "IRSensorRight: " << robotData.IRSensorRight << "\n";
-            outputFile << "IRSensorCenter: " << robotData.IRSensorCenter << "\n";
-            outputFile << "IRSensorLeft: " << robotData.IRSensorLeft << "\n";
-            outputFile << "GyroAngle: " << robotData.GyroAngle << "\n";
-            outputFile << "GyroAngleRate: " << robotData.GyroAngleRate << "\n";
-            outputFile << "CliffSensorRight: " << robotData.CliffSensorRight << "\n";
-            outputFile << "CliffSensorCenter: " << robotData.CliffSensorCenter << "\n";
-            outputFile << "CliffSensorLeft: " << robotData.CliffSensorLeft << "\n";
-            outputFile << "wheelCurrentLeft: " << robotData.wheelCurrentLeft << "\n";
-            outputFile << "wheelCurrentRight: " << robotData.wheelCurrentRight << "\n";
-            outputFile << "frameId: " << robotData.frameId << "\n";
-            outputFile << "HardwareVersionPatch: " << robotData.extraInfo.HardwareVersionPatch << "\n";
-            outputFile << "HardwareVersionMinor: " << robotData.extraInfo.HardwareVersionMinor << "\n";
-            outputFile << "HardwareVersionMajor: " << robotData.extraInfo.HardwareVersionMajor << "\n";
-            outputFile << "FirmwareVersionPatch: " << robotData.extraInfo.FirmwareVersionPatch << "\n";
-            outputFile << "FirmwareVersionMinor: " << robotData.extraInfo.FirmwareVersionMinor << "\n";
-            outputFile << "FirmwareVersionMajor: " << robotData.extraInfo.FirmwareVersionMajor << "\n";
-            outputFile << "UDID0: " << robotData.extraInfo.UDID0 << "\n";
-            outputFile << "UDID1: " << robotData.extraInfo.UDID1 << "\n";
-            outputFile << "UDID2: " << robotData.extraInfo.UDID2 << "\n";
-            outputFile.close();
-        }
-        else
-        {
-            std::cerr << "Error opening file\n";
-        }
-
-        std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds
+      std::cerr << "Error opening file\n";
     }
+
+    std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds
+  }
 }
 
-void sendIndividualKobukiData(const TKobukiData &data) {
-    while (true) {
-        client.publishMessage("kobuki/data/timestamp", std::to_string(data.timestamp));
-        client.publishMessage("kobuki/data/BumperCenter", std::to_string(data.BumperCenter));
-        client.publishMessage("kobuki/data/BumperLeft", std::to_string(data.BumperLeft));
-        client.publishMessage("kobuki/data/BumperRight", std::to_string(data.BumperRight));
-        client.publishMessage("kobuki/data/WheelDropLeft", std::to_string(data.WheelDropLeft));
-        client.publishMessage("kobuki/data/WheelDropRight", std::to_string(data.WheelDropRight));
-        client.publishMessage("kobuki/data/CliffCenter", std::to_string(data.CliffCenter));
-        client.publishMessage("kobuki/data/CliffLeft", std::to_string(data.CliffLeft));
-        client.publishMessage("kobuki/data/CliffRight", std::to_string(data.CliffRight));
-        client.publishMessage("kobuki/data/EncoderLeft", std::to_string(data.EncoderLeft));
-        client.publishMessage("kobuki/data/EncoderRight", std::to_string(data.EncoderRight));
-        client.publishMessage("kobuki/data/PWMleft", std::to_string(data.PWMleft));
-        client.publishMessage("kobuki/data/PWMright", std::to_string(data.PWMright));
-        client.publishMessage("kobuki/data/ButtonPress1", std::to_string(data.ButtonPress1));
-        client.publishMessage("kobuki/data/ButtonPress2", std::to_string(data.ButtonPress2));
-        client.publishMessage("kobuki/data/ButtonPress3", std::to_string(data.ButtonPress3));
-        client.publishMessage("kobuki/data/Charger", std::to_string(data.Charger));
-        client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery));
-        client.publishMessage("kobuki/data/overCurrent", std::to_string(data.overCurrent));
-        client.publishMessage("kobuki/data/IRSensorRight", std::to_string(data.IRSensorRight));
-        client.publishMessage("kobuki/data/IRSensorCenter", std::to_string(data.IRSensorCenter));
-        client.publishMessage("kobuki/data/IRSensorLeft", std::to_string(data.IRSensorLeft));
-        client.publishMessage("kobuki/data/GyroAngle", std::to_string(data.GyroAngle));
-        client.publishMessage("kobuki/data/GyroAngleRate", std::to_string(data.GyroAngleRate));
-        client.publishMessage("kobuki/data/CliffSensorRight", std::to_string(data.CliffSensorRight));
-        client.publishMessage("kobuki/data/CliffSensorCenter", std::to_string(data.CliffSensorCenter));
-        client.publishMessage("kobuki/data/CliffSensorLeft", std::to_string(data.CliffSensorLeft));
-        client.publishMessage("kobuki/data/wheelCurrentLeft", std::to_string(data.wheelCurrentLeft));
-        client.publishMessage("kobuki/data/wheelCurrentRight", std::to_string(data.wheelCurrentRight));
-        client.publishMessage("kobuki/data/digitalInput", std::to_string(data.digitalInput));
-        client.publishMessage("kobuki/data/analogInputCh0", std::to_string(data.analogInputCh0));
-        client.publishMessage("kobuki/data/analogInputCh1", std::to_string(data.analogInputCh1));
-        client.publishMessage("kobuki/data/analogInputCh2", std::to_string(data.analogInputCh2));
-        client.publishMessage("kobuki/data/analogInputCh3", std::to_string(data.analogInputCh3));
-        client.publishMessage("kobuki/data/frameId", std::to_string(data.frameId));
-        client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch", std::to_string(data.extraInfo.HardwareVersionPatch));
-        client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor", std::to_string(data.extraInfo.HardwareVersionMinor));
-        client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor", std::to_string(data.extraInfo.HardwareVersionMajor));
-        client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch", std::to_string(data.extraInfo.FirmwareVersionPatch));
-        client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMinor", std::to_string(data.extraInfo.FirmwareVersionMinor));
-        client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMajor", std::to_string(data.extraInfo.FirmwareVersionMajor));
-        client.publishMessage("kobuki/data/extraInfo/UDID0", std::to_string(data.extraInfo.UDID0));
-        client.publishMessage("kobuki/data/extraInfo/UDID1", std::to_string(data.extraInfo.UDID1));
-        client.publishMessage("kobuki/data/extraInfo/UDID2", std::to_string(data.extraInfo.UDID2));
+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",
+                          std::to_string(data.timestamp));
+    client.publishMessage("kobuki/data/BumperCenter",
+                          std::to_string(data.BumperCenter));
+    client.publishMessage("kobuki/data/BumperLeft",
+                          std::to_string(data.BumperLeft));
+    client.publishMessage("kobuki/data/BumperRight",
+                          std::to_string(data.BumperRight));
+    client.publishMessage("kobuki/data/WheelDropLeft",
+                          std::to_string(data.WheelDropLeft));
+    client.publishMessage("kobuki/data/WheelDropRight",
+                          std::to_string(data.WheelDropRight));
+    client.publishMessage("kobuki/data/CliffCenter",
+                          std::to_string(data.CliffCenter));
+    client.publishMessage("kobuki/data/CliffLeft",
+                          std::to_string(data.CliffLeft));
+    client.publishMessage("kobuki/data/CliffRight",
+                          std::to_string(data.CliffRight));
+    client.publishMessage("kobuki/data/EncoderLeft",
+                          std::to_string(data.EncoderLeft));
+    client.publishMessage("kobuki/data/EncoderRight",
+                          std::to_string(data.EncoderRight));
+    client.publishMessage("kobuki/data/PWMleft", std::to_string(data.PWMleft));
+    client.publishMessage("kobuki/data/PWMright",
+                          std::to_string(data.PWMright));
+    client.publishMessage("kobuki/data/ButtonPress1",
+                          std::to_string(data.ButtonPress1));
+    client.publishMessage("kobuki/data/ButtonPress2",
+                          std::to_string(data.ButtonPress2));
+    client.publishMessage("kobuki/data/ButtonPress3",
+                          std::to_string(data.ButtonPress3));
+    client.publishMessage("kobuki/data/Charger", std::to_string(data.Charger));
+    client.publishMessage("kobuki/data/Battery", std::to_string(data.Battery));
+    client.publishMessage("kobuki/data/overCurrent",
+                          std::to_string(data.overCurrent));
+    client.publishMessage("kobuki/data/IRSensorRight",
+                          std::to_string(data.IRSensorRight));
+    client.publishMessage("kobuki/data/IRSensorCenter",
+                          std::to_string(data.IRSensorCenter));
+    client.publishMessage("kobuki/data/IRSensorLeft",
+                          std::to_string(data.IRSensorLeft));
+    client.publishMessage("kobuki/data/GyroAngle",
+                          std::to_string(data.GyroAngle));
+    client.publishMessage("kobuki/data/GyroAngleRate",
+                          std::to_string(data.GyroAngleRate));
+    client.publishMessage("kobuki/data/CliffSensorRight",
+                          std::to_string(data.CliffSensorRight));
+    client.publishMessage("kobuki/data/CliffSensorCenter",
+                          std::to_string(data.CliffSensorCenter));
+    client.publishMessage("kobuki/data/CliffSensorLeft",
+                          std::to_string(data.CliffSensorLeft));
+    client.publishMessage("kobuki/data/wheelCurrentLeft",
+                          std::to_string(data.wheelCurrentLeft));
+    client.publishMessage("kobuki/data/wheelCurrentRight",
+                          std::to_string(data.wheelCurrentRight));
+    client.publishMessage("kobuki/data/digitalInput",
+                          std::to_string(data.digitalInput));
+    client.publishMessage("kobuki/data/analogInputCh0",
+                          std::to_string(data.analogInputCh0));
+    client.publishMessage("kobuki/data/analogInputCh1",
+                          std::to_string(data.analogInputCh1));
+    client.publishMessage("kobuki/data/analogInputCh2",
+                          std::to_string(data.analogInputCh2));
+    client.publishMessage("kobuki/data/analogInputCh3",
+                          std::to_string(data.analogInputCh3));
+    client.publishMessage("kobuki/data/frameId", std::to_string(data.frameId));
+    client.publishMessage("kobuki/data/extraInfo/HardwareVersionPatch",
+                          std::to_string(data.extraInfo.HardwareVersionPatch));
+    client.publishMessage("kobuki/data/extraInfo/HardwareVersionMinor",
+                          std::to_string(data.extraInfo.HardwareVersionMinor));
+    client.publishMessage("kobuki/data/extraInfo/HardwareVersionMajor",
+                          std::to_string(data.extraInfo.HardwareVersionMajor));
+    client.publishMessage("kobuki/data/extraInfo/FirmwareVersionPatch",
+                          std::to_string(data.extraInfo.FirmwareVersionPatch));
+    client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMinor",
+                          std::to_string(data.extraInfo.FirmwareVersionMinor));
+    client.publishMessage("kobuki/data/extraInfo/FirmwareVersionMajor",
+                          std::to_string(data.extraInfo.FirmwareVersionMajor));
+    client.publishMessage("kobuki/data/extraInfo/UDID0",
+                          std::to_string(data.extraInfo.UDID0));
+    client.publishMessage("kobuki/data/extraInfo/UDID1",
+                          std::to_string(data.extraInfo.UDID1));
+    client.publishMessage("kobuki/data/extraInfo/UDID2",
+                          std::to_string(data.extraInfo.UDID2));
 
-        if (!data.gyroData.empty()) {
-            const auto& latestGyro = data.gyroData.back();
-            client.publishMessage("kobuki/data/gyroData/x", std::to_string(latestGyro.x));
-            client.publishMessage("kobuki/data/gyroData/y", std::to_string(latestGyro.y));
-            client.publishMessage("kobuki/data/gyroData/z", std::to_string(latestGyro.z));
-        }
-
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    if (!data.gyroData.empty())
+    {
+      const auto &latestGyro = data.gyroData.back();
+      client.publishMessage("kobuki/data/gyroData/x",
+                            std::to_string(latestGyro.x));
+      client.publishMessage("kobuki/data/gyroData/y",
+                            std::to_string(latestGyro.y));
+      client.publishMessage("kobuki/data/gyroData/z",
+                            std::to_string(latestGyro.z));
     }
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  }
 }
 
-std::string serializeKobukiData(const TKobukiData &data) {
-    std::string json = "{\"timestamp\":" + std::to_string(data.timestamp) +
-                       ",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
-                       ",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
-                       ",\"BumperRight\":" + std::to_string(data.BumperRight) +
-                       ",\"WheelDropLeft\":" + std::to_string(data.WheelDropLeft) +
-                       ",\"WheelDropRight\":" + std::to_string(data.WheelDropRight) +
-                       ",\"CliffCenter\":" + std::to_string(data.CliffCenter) +
-                       ",\"CliffLeft\":" + std::to_string(data.CliffLeft) +
-                       ",\"CliffRight\":" + std::to_string(data.CliffRight) +
-                       ",\"EncoderLeft\":" + std::to_string(data.EncoderLeft) +
-                       ",\"EncoderRight\":" + std::to_string(data.EncoderRight) +
-                       ",\"PWMleft\":" + std::to_string(data.PWMleft) +
-                       ",\"PWMright\":" + std::to_string(data.PWMright) +
-                       ",\"ButtonPress1\":" + std::to_string(data.ButtonPress1) +
-                       ",\"ButtonPress2\":" + std::to_string(data.ButtonPress2) +
-                       ",\"ButtonPress3\":" + std::to_string(data.ButtonPress3) +
-                       ",\"Charger\":" + std::to_string(data.Charger) +
-                       ",\"Battery\":" + std::to_string(data.Battery) +
-                       ",\"overCurrent\":" + std::to_string(data.overCurrent) +
-                       ",\"IRSensorRight\":" + std::to_string(data.IRSensorRight) +
-                       ",\"IRSensorCenter\":" + std::to_string(data.IRSensorCenter) +
-                       ",\"IRSensorLeft\":" + std::to_string(data.IRSensorLeft) +
-                       ",\"GyroAngle\":" + std::to_string(data.GyroAngle) +
-                       ",\"GyroAngleRate\":" + std::to_string(data.GyroAngleRate) +
-                       ",\"CliffSensorRight\":" + std::to_string(data.CliffSensorRight) +
-                       ",\"CliffSensorCenter\":" + std::to_string(data.CliffSensorCenter) +
-                       ",\"CliffSensorLeft\":" + std::to_string(data.CliffSensorLeft) +
-                       ",\"wheelCurrentLeft\":" + std::to_string(data.wheelCurrentLeft) +
-                       ",\"wheelCurrentRight\":" + std::to_string(data.wheelCurrentRight) +
-                       ",\"digitalInput\":" + std::to_string(data.digitalInput) +
-                       ",\"analogInputCh0\":" + std::to_string(data.analogInputCh0) +
-                       ",\"analogInputCh1\":" + std::to_string(data.analogInputCh1) +
-                       ",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) +
-                       ",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) +
-                       ",\"frameId\":" + std::to_string(data.frameId) +
-                       ",\"extraInfo\":{\"HardwareVersionPatch\":" + std::to_string(data.extraInfo.HardwareVersionPatch) +
-                       ",\"HardwareVersionMinor\":" + std::to_string(data.extraInfo.HardwareVersionMinor) +
-                       ",\"HardwareVersionMajor\":" + std::to_string(data.extraInfo.HardwareVersionMajor) +
-                       ",\"FirmwareVersionPatch\":" + std::to_string(data.extraInfo.FirmwareVersionPatch) +
-                       ",\"FirmwareVersionMinor\":" + std::to_string(data.extraInfo.FirmwareVersionMinor) +
-                       ",\"FirmwareVersionMajor\":" + std::to_string(data.extraInfo.FirmwareVersionMajor) +
-                       ",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) +
-                       ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
-                       ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
+std::string serializeKobukiData(const TKobukiData &data)
+{
+  std::string json =
+      "{\"timestamp\":" + std::to_string(data.timestamp) +
+      ",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
+      ",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
+      ",\"BumperRight\":" + std::to_string(data.BumperRight) +
+      ",\"WheelDropLeft\":" + std::to_string(data.WheelDropLeft) +
+      ",\"WheelDropRight\":" + std::to_string(data.WheelDropRight) +
+      ",\"CliffCenter\":" + std::to_string(data.CliffCenter) +
+      ",\"CliffLeft\":" + std::to_string(data.CliffLeft) +
+      ",\"CliffRight\":" + std::to_string(data.CliffRight) +
+      ",\"EncoderLeft\":" + std::to_string(data.EncoderLeft) +
+      ",\"EncoderRight\":" + std::to_string(data.EncoderRight) +
+      ",\"PWMleft\":" + std::to_string(data.PWMleft) +
+      ",\"PWMright\":" + std::to_string(data.PWMright) +
+      ",\"ButtonPress1\":" + std::to_string(data.ButtonPress1) +
+      ",\"ButtonPress2\":" + std::to_string(data.ButtonPress2) +
+      ",\"ButtonPress3\":" + std::to_string(data.ButtonPress3) +
+      ",\"Charger\":" + std::to_string(data.Charger) +
+      ",\"Battery\":" + std::to_string(data.Battery) +
+      ",\"overCurrent\":" + std::to_string(data.overCurrent) +
+      ",\"IRSensorRight\":" + std::to_string(data.IRSensorRight) +
+      ",\"IRSensorCenter\":" + std::to_string(data.IRSensorCenter) +
+      ",\"IRSensorLeft\":" + std::to_string(data.IRSensorLeft) +
+      ",\"GyroAngle\":" + std::to_string(data.GyroAngle) +
+      ",\"GyroAngleRate\":" + std::to_string(data.GyroAngleRate) +
+      ",\"CliffSensorRight\":" + std::to_string(data.CliffSensorRight) +
+      ",\"CliffSensorCenter\":" + std::to_string(data.CliffSensorCenter) +
+      ",\"CliffSensorLeft\":" + std::to_string(data.CliffSensorLeft) +
+      ",\"wheelCurrentLeft\":" + std::to_string(data.wheelCurrentLeft) +
+      ",\"wheelCurrentRight\":" + std::to_string(data.wheelCurrentRight) +
+      ",\"digitalInput\":" + std::to_string(data.digitalInput) +
+      ",\"analogInputCh0\":" + std::to_string(data.analogInputCh0) +
+      ",\"analogInputCh1\":" + std::to_string(data.analogInputCh1) +
+      ",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) +
+      ",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) +
+      ",\"frameId\":" + std::to_string(data.frameId) +
+      ",\"extraInfo\":{\"HardwareVersionPatch\":" +
+      std::to_string(data.extraInfo.HardwareVersionPatch) +
+      ",\"HardwareVersionMinor\":" +
+      std::to_string(data.extraInfo.HardwareVersionMinor) +
+      ",\"HardwareVersionMajor\":" +
+      std::to_string(data.extraInfo.HardwareVersionMajor) +
+      ",\"FirmwareVersionPatch\":" +
+      std::to_string(data.extraInfo.FirmwareVersionPatch) +
+      ",\"FirmwareVersionMinor\":" +
+      std::to_string(data.extraInfo.FirmwareVersionMinor) +
+      ",\"FirmwareVersionMajor\":" +
+      std::to_string(data.extraInfo.FirmwareVersionMajor) +
+      ",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) +
+      ",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
+      ",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
 
-    if (!data.gyroData.empty()) {
-        const auto& latestGyro = data.gyroData.back();
-        json += "{\"x\":" + std::to_string(latestGyro.x) +
-                ",\"y\":" + std::to_string(latestGyro.y) +
-                ",\"z\":" + std::to_string(latestGyro.z) + "}";
-    }
+  if (!data.gyroData.empty())
+  {
+    const auto &latestGyro = data.gyroData.back();
+    json += "{\"x\":" + std::to_string(latestGyro.x) +
+            ",\"y\":" + std::to_string(latestGyro.y) +
+            ",\"z\":" + std::to_string(latestGyro.z) + "}";
+  }
 
-    json += "]}";
-    return json;
+  json += "]}";
+  return json;
 }
-//create extra function to send the message every 100ms
-//needed it so it can be threaded
-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));
-    }
+
+// create extra function to send the message every 100ms
+// needed it so it can be threaded
+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));
+  }
 }
 
 void CapnSend() {
-    int fps = 15;
-    int width = 800;
-    int height = 600;
-
-    VideoCapture cap("/dev/video0");
+  int COMPRESSION_LEVEL = 90;
+    VideoCapture cap(0); // Open the camera
     if (!cap.isOpened()) {
         cerr << "Error: Could not open camera" << endl;
         return;
     }
 
-    VideoWriter out("appsrc ! videoconvert ! video/x-raw,format=I420 ! x264enc speed-preset=ultrafast bitrate=600 key-int-max=" + to_string(fps * 2) + " ! video/x-h264,profile=baseline ! mpegtsmux ! udpsink host=127.0.0.1 port=5001",
-                    CAP_GSTREAMER, 0, fps, Size(width, height), true);
-    if (!out.isOpened()) {
-        cerr << "Error: Can't open video writer" << endl;
-        return;
-    }
-
     Mat frame;
     while (true) {
-        cap >> frame; // Capture a new image frame
-        if (frame.empty()) {
-            cerr << "Error: Could not capture image" << endl;
-            continue;
+        if (!cap.read(frame)) { 
+            cout << "Reconnecting camera" << endl;
+            cap.release();
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+            // Attempt to reconnect to the camera
+            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
+                continue;
+            } else {
+                cout << "Reconnected to camera" << endl;
+                continue;
+            }
         }
 
-        // Write the frame to the UDP stream
-        out.write(frame);
-        cout << "Sent image" << endl;
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / fps)); // Control the frame rate
+        vector<uchar> imgbuf;
+        vector<int> compression_params;
+        compression_params.push_back(IMWRITE_JPEG_QUALITY); // Set JPEG quality
+        compression_params.push_back(COMPRESSION_LEVEL);  // Adjust the quality level (0-100, lower = more compression)
+
+        // Encode the image into the byte buffer with the specified compression parameters
+        imencode(".jpg", frame, imgbuf, compression_params);
+
+        // Convert the vector<uchar> buffer to a string (no casting)
+        string enc_msg(imgbuf.begin(), imgbuf.end());
+
+        // Publish the compressed image data (MQTT, in this case)
+        client.publishMessage("kobuki/cam", enc_msg);
+        cout << "Sent compressed image" << endl;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Send image every 200ms
     }
 }

src/Python/flask/Dockerfile

@@ -14,5 +14,5 @@ EXPOSE 5000
 CMD ["python", "web/app.py"]
 
 #build instruction: sudo docker buildx build -t flaskapp:latest .   
-#run instruction: sudo docker run --network="host" flaskapp:latest
+#run instruction: sudo docker run --network="host" --restart=always flaskapp:latest
 # need to use network host to connect to the host's mqtt server

src/Python/flask/requirements.txt

@@ -3,3 +3,4 @@ paho-mqtt==1.6.1
 ultralytics==8.3.58 
 opencv-python-headless==4.6.0.66
 numpy==1.23.4
+mysql-connector-python==9.1.0

src/Python/flask/web/app.py

@@ -1,9 +1,11 @@
-from flask import Flask, Response, request, render_template, jsonify
+from flask import Flask, Response, request, render_template, jsonify, g
 import paho.mqtt.client as mqtt
 from ultralytics import YOLO
 import cv2
 import numpy as np
 import threading
+import mysql.connector 
+import json
 
 app = Flask(__name__)
 
@@ -57,19 +59,37 @@ mqtt_client.subscribe("kobuki/cam")
 
 mqtt_client.on_message = on_message  # this line needs to be under the function definition otherwise it can't find which function it needs to use
 
+# Database connectie-functie
+def get_db():
+    if 'db' not in g:  # 'g' is specifiek voor een request en leeft zolang een request duurt
+        g.db = mysql.connector.connect(
+            host="127.0.0.1",
+            port=3306,
+            user="admin",
+            password="kobuki",
+            database="kobuki"
+        )
+    return g.db
+
+# Sluit de database na elke request
+@app.teardown_appcontext
+def close_db(error):
+    db = g.pop('db', None)
+    if db is not None:
+        db.close()
+
 @app.route('/')
 def index():
     return render_template('index.html')
 
-
 @app.route('/control', methods=["GET", "POST"])
 def control():
     if request.authorization and request.authorization.username == 'ishak' and request.authorization.password == 'kobuki':
+        yolo_results_db()
         return render_template('control.html')
     else:
         return ('Unauthorized', 401, {'WWW-Authenticate': 'Basic realm="Login Required"'})
 
-
 @app.route('/move', methods=['POST'])
 def move():
     data = request.get_json()
@@ -77,14 +97,55 @@ def move():
 
     # Verstuur de richting via MQTT
     if direction:
-        mqtt_client.publish("home/commands", direction)  # Het topic kan aangepast worden
+        mqtt_client.publish("home/commands", direction)
 
+    db_connection = get_db()
+    cursor = db_connection.cursor()
+    sql_command = "INSERT INTO command (command) VALUES (%s)"
+    cursor.execute(sql_command, (direction,))        
+    
+    db_connection.commit()
+    cursor.close()
+    db_connection.close()
     return jsonify({"status": "success", "direction": direction})
-
+    
 
 @app.route('/data', methods=['GET'])
 def data():
+    try:
+        # Parse de JSON-string naar een Python-dictionary
+        data = json.loads(kobuki_message)
+
+        # Maak een lijst van tuples met de naam en waarde van elk veld
+        sensor_data_tuples = [(name, float(value)) for name, value in data.items() if isinstance(value, (int, float))]
+
+        # Extra informatie of nested data (zoals "extraInfo" of "gyroData") kun je apart verwerken
+        if "extraInfo" in data:
+            for key, value in data["extraInfo"].items():
+                sensor_data_tuples.append((f"extraInfo_{key}", float(value)))
+
+        if "gyroData" in data:
+            for i, gyro in enumerate(data["gyroData"]):
+                for axis, value in gyro.items():
+                    sensor_data_tuples.append((f"gyroData_{i}_{axis}", float(value)))
+
+        # Database-insert
+        db = get_db()
+        with db.cursor() as cursor:
+
+            # Zorg dat je tabel `kobuki_data` kolommen heeft: `name` en `value`
+            sql_sensor = "INSERT INTO kobuki_data (name, value) VALUES (%s, %s)"
+            cursor.executemany(sql_sensor, sensor_data_tuples)
+    
+            # Commit en sluit de cursor
+            db.commit()
+            cursor.close()
+    except json.JSONDecodeError as e:
+        print(f"JSON decode error: {e}")
+    except mysql.connector.Error as err:
+        print(f"Database error: {err}")
     return kobuki_message
+    
 
 
 @app.route('/image')
@@ -100,10 +161,25 @@ def image():
 
 @app.route('/yolo_results', methods=['GET'])
 def yolo_results_endpoint():
-    global yolo_results
-    with lock:
-        return jsonify(yolo_results)
+    global yolo_results        
+    return jsonify(yolo_results)
 
+def yolo_results_db():
+    global yolo_results    
+    with lock:
+        try:
+            db = get_db()
+            with db.cursor() as cursor:
+                sql_yolo = "INSERT INTO image (class, confidence) VALUES (%s, %s)"
+                yolo_tuples = [(result["class"], result["confidence"]) for result in yolo_results]
+                print(f"YOLO Tuples: {yolo_tuples}")  # Debug statement
+                cursor.executemany(sql_yolo, yolo_tuples)
+                db.commit()
+                cursor.close()
+        except mysql.connector.Error as err:
+            print(f"Database error: {err}")
+        except Exception as e:
+            print(f"Unexpected error: {e}")
 
 if __name__ == '__main__':
     app.run(debug=True, port=5000)

src/Python/flask/web/static/script.js

@@ -43,19 +43,21 @@ document.addEventListener("DOMContentLoaded", function() {
     for (const [key, value] of Object.entries(data)) {
       const dataElement = document.createElement("p");
       dataElement.textContent = `${key}: ${value}`;
-      sensorDataContainer.appendChild(dataElement);
+      sensorDataContainer.appendChild(dataElement); // Add the element to the container
     }
   }
 
   // Update the image
-  function updateImage() {
-    var img = document.getElementById("robot-image");
+  async function updateImage() {
+    let img = document.getElementById("robot-image");
     img.src = "/image?" + new Date().getTime(); // Add timestamp to avoid caching
+    // Wait for 200 milliseconds before fetching the next image
+    setTimeout(updateImage, 200);
   }
 
   // Fetch and display sensor data every 1 second
   setInterval(parseData, 1000);
 
-  // Update the image every 200 milliseconds
-  setInterval(updateImage, 100);
+  // Start updating the image
+  updateImage();
 });

teamdocumentatie/Ishak/etische_aspecten.md

@@ -0,0 +1,25 @@
+#  Etische aspecten van het project
+
+## Visie op de ethische aspecten van het Kobuki-project
+
+Etische aspecten zijn heel belangrijk in het project, al ben ik wel van mening dat je niet alles kan voorkomen en ook kan waarborgen.
+
+## Privacy
+
+Als je bijvoorbeeld kijkt naar het gedeelte privacy, dan is het heel moeilijk om te kijken wat je gaat doen met die gegevens. Ik ga een camera gebruiken op de robot om zo te kijken 
+waar de robot is en wat hij allemaal ziet. Als de robot in een brandende huis komt en dan een persoon ziet, is het wel belangrijk om die persoon goed te kunnen zien. Je zou dan niet kunnen zeggen dat je die persoon bijvoorbeeld moet vervagen, want je moet wel kunnen zien wat de status is van die persoon.
+Ook is het dan belangrijk om te kijken wat je met die gegevens gaat doen, ga je ze opslaan voor eventuele later gebruik of verwijder je ze direct. Het is heel lastig te bepalen wanneer je op zo een moment privacy schendt. 
+
+## Betrouwbaarheid
+
+Ik vind dat je de betrouwbaarheid van de robot wel moet waarborgen,
+want als ik de robot in een brandend huis stuur en hij valt uit, dan kan dat heel gevaarlijk zijn voor de persoon die in dat huis zit. Daar vind ik dat je meer rekening mee moet houden dan met de privacy van de persoon. Het is de bedoeling dat de robot hulpmedewerkers gaat helpen en niet hun werk moeilijker maakt.
+
+## Impact op hulpverleners & maatschappij
+
+Als meerdere hulpmedewerkers de robot gaan gebruiken en het word een soort van standaard, dan is het wel belangrijk dat de robot betrouwbaar is en dat je erop kan vertrouwen. Het gaat immers om mensenlevens en dat is wel het belangrijkste. Het is uiteindelijk de bedoeling dat de robot hulpverleners zal helpen en niet hun werk lastiger moet maken. Als de robot een standaard hulpmiddel wordt moet hij wel gebruiksvriendelijk zijn en goed kunnen helpen. De robot moet ook zo goed mogelijk werken om zo de vertrouwen te behouden van de mensen. Als de robot fouten blijft maken en niet betrouwbaar is zullen minder mensen het gebruiken. Ik vind dan ook dat de gebruik van de robot heel transparant moet zijn. Hoe word de robot aangestuurd, hoe vergelijkt hij situaties en hoe hij daarmee omgaat. 
+Als je daar al heel duidelijk in bent bouw je al wat sneller vertrouwen van de mensen op. 
+
+Ik vind dat in dit project de ethische aspecten heel belangrijk zijn en dat je daar ook rekening mee moet houden. 
+Bij betrouwbaarheid en de impact die de robot kan hebben op de maatschappij en de hulpverleners moet je wel goed over nadenken. 
+Je werkt immers met mensenlevens en dat is wel het belangrijkste. Privacy is ook heel belangrijk, maar ik vind dat je daar wel wat soepeler mee om kan gaan.

teamdocumentatie/Ishak/expert.md

@@ -0,0 +1,7 @@
+# feedback
+
+- schrijf altijd in je code waarvan je de library importeert. Dit is handig voor jezelf en voor anderen die je code lezen.
+- Meer comments in je code zouden handig zijn om te begrijpen wat je code doet.
+- Database: timestamp voor wanneer je een record toevoegt in je command.
+- Kobuki sensor data ook opslaan in de database.
+- 

teamdocumentatie/Ishak/motivatie.md

@@ -0,0 +1,21 @@
+Motivation Letter
+
+16/12/2024
+
+Cognizant Digital StudioAttn. Hayo Rubingh
+
+Subject: Internship Application Cognizant Digital Studio
+
+Dear Mr. Rubingh,
+
+With great enthusiasm, I am applying for the internship position at Cognizant Digital Studio in Amsterdam. As a second-year bachelor’s student in Technische Informatica(Computer Science) at Hogeschool Van Amsterdam, I am seeking a challenging internship where I can combine my technical skills with my passion for innovation. Cognizant’s focus  IoT, and technology prototypes fits perfectly with my interests .
+
+Throughout my studies, I have gained experience in software development, including Python and JavaScript, and have worked with IoT devices such as Arduino/ESP. What drives me is the opportunity to create and develop new solutions that can make life easier and more efficient. I am particularly interested in the field of IoT and the possibilities it offers for creating smart solutions. I am eager to learn more about the latest technologies and how they can be applied in real-world projects.
+
+I am available to start in February 2025 and look forward to contributing to innovative projects.
+
+I would be delighted to discuss my motivation and experience further in a personal interview. You can find my contact details in my CV. Thank you for considering my application. I am looking forward to hearing from you.
+
+Yours sincerely,
+
+Ishak Jmilou

teamdocumentatie/Ishak/verslag.md

@@ -0,0 +1,64 @@
+![alt text](images/image.png)
+
+# Welke communicatieprotocol geeft de mogelijkheid om veilig en betrouwbaar te communiceren tussen IoT apparaten?
+
+Auteur: Ishak Jmilou
+
+Datum: 17-12-2024
+
+
+
+---
+
+
+## Inleiding
+
+In dit verslag wordt er gekeken naar de verschillende communicatieprotocollen die gebruikt kunnen worden om veilig en betrouwbaar te communiceren tussen IoT apparaten. Er wordt gekeken naar de verschillende protocollen en de voor- en nadelen van elk protocol.
+
+---
+
+# Samenvatting
+
+In dit verslag worden de communicatieprotocollen MQTT, HTTP, WebSockets en CoAP vergeleken op het gebied van veilige en betrouwbare communicatie tussen IoT-apparaten. Veilige communicatie omvat gegevensversleuteling en authenticatie, terwijl betrouwbaarheid verwijst naar het verzenden en ontvangen van gegevens zonder verlies. Op basis van deze overwegingen wordt MQTT aanbevolen voor situaties waar zowel veiligheid als betrouwbaarheid cruciaal zijn in IoT-communicatie.
+
+## 1. Wat houdt veilige en betrouwbare communicatie tussen apparaten in?
+
+Als je werkt met IoT-apparaten, is het belangrijk dat de communicatie tussen deze apparaten veilig en betrouwbaar is. Iot-apparaten verzamelen gegevens over de omgeving en communiceert deze tussen apparaten over het internet. Als deze communicatie niet veilig is, kunnen hackers deze gegevens onderscheppen en gebruiken(Ministerie van Algemene Zaken, 2022). Je wilt voorkomen dat hackers toegang krijgen tot gevoelige informatie zoals persoonlijke gegevens of bedrijfsgeheimen. Daarom is het belangrijk dat de communicatie tussen apparaten veilig en betrouwbaar is. Een protocol is een set regels die bepalen hoe apparaten met elkaar communiceren. Er zijn verschillende protocollen die gebruikt kunnen worden om veilig en betrouwbaar te communiceren tussen IoT-apparaten.
+
+## 2. Welke protocollen zijn er om veilig en betrouwbaar te communiceren tussen apparaten?
+
+Een communicatieprotocol is een set regels die bepalen hoe apparaten met elkaar communiceren(Paul Christiano, 2023). Er is voor elk project een ander protocol dat het beste past. In dit geval is het belangrijk dat de communicatie veilig en betrouwbaar is. De protocollen die ik ga vergelijken zijn MQTT, HTTP, WebSockets en CoAP. Wat belangrijk is om te onderzoeken is hoe de protocollen omgaan met veiligheid en betrouwbaarheid. Veiligheid kan worden bereikt door gegevens te versleutelen en te authenticeren. Betrouwbaarheid kan worden bereikt door gegevens te verzenden en te ontvangen zonder verlies.(Paul Christiano, 2023).
+
+## 3. Wat zijn de voor- en nadelen van de verschillende protocollen?
+
+![alt text](image.png)
+
+Zoals te zien is in de tabel is CoAP minder betrouwbaar dan de andere protocollen. Dit komt, omdat CoAP gebruik maakt van UDP wat ervoor zorgt dat het sneller is maar niet betrouwbaar met de berichten die hij stuurt(Darek Fanton, 2023). Websockets is een goede protocol alleen is het niet altijd geschikt voor lichtgewicht apparaten. HTTP maakt gebruik van TCP wat betrouwbaar is, maar het nadeel van HTTP is dat het elke keer een nieuwe verbinding moet maken. Dit kan een probleem zijn als je veel berichten moet versturen. MQTT is een lichtgewicht protocol dat betrouwbaar is en verschillende niveaus van kwaliteit van de berichten ondersteunt. Het is ook mogelijk om gegevens te versleutelen en te authenticeren met MQTT. Dit maakt het een goede keuze voor veilige en betrouwbare communicatie tussen IoT-apparaten.
+
+## 4. Conclusie
+
+Er zijn verschillende protocollen die goed gebruikt kunnen worden voor IoT apparaten. Aangezien voor mijn project veiligheid en betrouwbaarheid op één staat heb ik gekozen voor MQTT. Dit protocol is lichtgewicht, betrouwbaar en ondersteunt verschillende niveaus van kwaliteit van de berichten. Het is ook mogelijk om gegevens te versleutelen en te authenticeren met MQTT. Dit maakt het een goede keuze voor veilige en betrouwbare communicatie tussen IoT-apparaten.
+
+## literatuurlijst
+
+- Singh, S., & Jyoti. (2024, June 7). Secure Communications Protocols for IoT networks: a survey. https://journal.ijprse.com/index.php/ijprse/article/view/1082
+  
+- Nguyen, K. T., Laurent, M., Oualha, N., CEA, & Institut Mines-Telecom. (2015). Survey on secure communication protocols for the Internet of Things. In Ad Hoc Networks (Vol. 32, pp. 17–31) [Journal-article]. http://dx.doi.org/10.1016/j.adhoc.2015.01.006
+
+- Miorandi, D., Sicari, S., De Pellegrini, F., & Imrich Chlamtac. (2012). Internet of things: Vision, applications and research challenges. In Ad Hoc Networks (Vol. 10, pp. 1497–1516) [Journal-article]. Elsevier B.V. http://dx.doi.org/10.1016/j.adhoc.2012.02.016
+
+- Christiano, P. (2023, November 5). Top 9 IoT communication protocols & their features in 2024: An In-Depth guide - ExpertBeacon. Expertbeacon. https://expertbeacon.com/iot-communication-protocol/
+
+- Yugha, R., & Chithra, S. (2020). A survey on technologies and security protocols: Reference for future generation IoT. Journal of Network and Computer Applications, 169, 102763. https://doi.org/10.1016/j.jnca.2020.102763
+
+- De Mendizábal, I. (2022, June 16). IoT Communication Protocols—IoT Data Protocols. Technical Articles. https://www.allaboutcircuits.com/technical-articles/internet-of-things-communication-protocols-iot-data-protocols/
+
+- IoT-technologieën en -protocollen | Microsoft Azure. (n.d.). https://azure.microsoft.com/nl-nl/solutions/iot/iot-technology-protocols
+
+- Darek Fanton(2024, Juli 11). Het IoT verbinden: wat is MQTT en waarin verschilt het van CoAP? (n.d.). https://www.onlogic.com/nl/blog/het-iot-verbinden-wat-is-mqtt-en-waarin-verschilt-het-van-coap/
+
+- Nader, K. (2023, October 30). Wat zijn de voordelen van het gebruik van WebSocket voor IoT-communicatie? AppMaster - Ultimate All-in No-code Platform. https://appmaster.io/nl/blog/websocket-voor-iot-communicatie
+
+- Sidna, J., Amine, B., Abdallah, N., & Alami, H. E. (2020). Analysis and evaluation of communication Protocols for IoT Applications. Karbala International Journal of Modern Science. https://doi.org/10.1145/3419604.3419754
+
+- Ministerie van Algemene Zaken. (2022, February 8). Hoe kan ik slimme apparaten veilig gebruiken? Rijksoverheid.nl. https://www.rijksoverheid.nl/onderwerpen/bescherming-van-consumenten/vraag-en-antwoord/hoe-kan-ik-slimme-apparaten-veilig-gebruiken