Code toegevoegd voor de sgp32 sensor zodat deze wordt geïnitialiseerd

Removed callback line
Removed network information
2025-08-05 12:54:57 +00:00 · 2025-01-14 17:43:27 +01:00 · 2024-12-18 15:07:02 +01:00 · 2024-12-18 13:23:13 +01:00 · 2024-12-17 17:10:40 +01:00 · 2024-12-17 16:51:50 +01:00
40 changed files with 1409 additions and 1340 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -16,11 +16,6 @@ src/C++/Driver/Makefile
 src/C++/Driver/vgcore*
 src/C++/Driver/cmake_install.cmake
 src/C++/Driver/Makefile
-src/Python/flask/web/_pycache_
-venv
+src/C++/Driver/log
 build/
-log
-CMakeFiles/
-Makefile
-CMakeCache.txt
-cmake_install.cmake
+venv
--- a/.gitlab/issue_templates/Learning
+++ b/.gitlab/issue_templates/Learning
@@ -0,0 +1,21 @@
+```
+Inleiding van de learning story:
+Vul hier een korte beschrijving in van de learning story. 
+Probeer dit direct te koppelen aan het project wat studenten gaan doen.
+```
+
+**Wat ga ik leren?**
+
+```
+Zet hier een overzicht van de vaardigheden die een student gaat leren. 
+Als de student alle vinkjes heeft afgevinkt, dan hebben ze dit onderwerp goed begrepen.
+```
+
+- [ ] <- Maak op deze manier afvinklijstjes
+
+**Hoe ga ik dit leren?**
+
+```
+Zet hier de bronnenlijst.
+```
+- [ ] <- Maak op deze manier afvinklijstjes
--- a/.gitlab/issue_templates/User
+++ b/.gitlab/issue_templates/User
@@ -0,0 +1,32 @@
+Als gebruiker wil ik ..., zodat ... (verwijder deze regel, plaats de tekst in de titel)
+
+
+[beschrijving van de user story en context]
+
+**Taken**
+
+Deze user story is opgedeeld in een aantal taken. Vul onderstaande lijst zelf aan.
+
+- [ ] Taak 1.
+- [ ] Taak 2.
+- [ ] ...
+
+**Acceptatie criteria**
+
+Acceptatie criteria zijn specifieke eisen waaraan de User Story moet voldoen. Deze zijn meestal uniek per User Story.
+
+- [ ] Acceptatiecriterium 1 
+- [ ] Acceptatiecriterium 2 
+- [ ] ...
+
+**Definition of Done**
+
+- [ ] Alle acceptatiecriteria van de user story zijn afgevinkt.
+- [ ] Je hebt volgens de HBO-ICT werkstandaarden gewerkt (Agile, GitLab, sprint boards, sprint planning, HBO-ICT conventions etc.)
+- [ ] Het werk is technisch gedocumenteerd in het Engels en relevant voor collega-ontwikkelaars. Denk o.a. aan ERD, UML, testen en testresultaten.
+- [ ] Het leerproces is beschreven in Standaardnederlands.
+- [ ] Het werk is gereviewd door een peer.
+- [ ] Het UX/UI gedeelte van de applicatie voldoet aan het Think-Make-Check (TMC) principe.
+- [ ] De code is functioneel getest op fouten.
+- [ ] De code werkt zonder fouten bij normaal gebruik.
+- [ ] De webapplicatie dient zowel op mobiele- als desktop-apparaten gebruikt te kunnen worden.
--- a/docs/Infrastructure/infrastructure.md
+++ b/docs/Infrastructure/infrastructure.md
@@ -2,35 +2,51 @@

 ```mermaid
 classDiagram
+Apache <--> Flask
+Flask <--> MariaDB
+ESP32 --> RPI : Wired communication
+RPI <--> Wireguard : MQTT with a VPN connection
+Wireguard <--> Flask
+

-VirtualMachine <--> RPI
-Kobuki <--> RPI
 namespace Server {
-    class VirtualMachine {
-        +Apache()
-        +Flask()
-        +SocketServer()
-        +MariaDB()
-        Python/C++
-        Database
-        Website
-

+    class Wireguard {
+        +VPN()
    }
+
+    class Apache {
+        +ReverseProxy()
+    }
+
+    class MariaDB {
+        +Database()
+    }
+
+    class Flask {
+        Python
+        +Webserver()
+        +endpoints communication()
+    }
+
+
 }
-namespace robot {
-    class RPI {
-    Receiver
-    Sensors
-    C++
-    +SocketClient()
-    +Kobuki()

-    }
+    namespace Kobuki {
+        class RPI {
+            +KobukiCommunication()
+            +ESP32Communication()
+            C++
+            }

-    class Kobuki {
-        +data
-    }
- 
+        class ESP32 {
+            +TVOC()
+            +ECO2()
+            +Temperature()
+            +LDR()
+            +Camera()
+            +GPS()
+            +ToF()
+        }
 }
-```
+```
--- a/docs/Ishak/feedback.md
+++ b/docs/Ishak/feedback.md
@@ -1,2 +0,0 @@
-# Feedback expert review 
-
--- a/docs/Ishak/smartleerdoel.md
+++ b/docs/Ishak/smartleerdoel.md
@@ -1,49 +0,0 @@
-# Smart leerdoelen
-
-Na de retrospective die ik heb gedaan, heb ik besloten om de volgende smart leerdoelen te stellen:
-
-1 **Ik wil in de volgende blok meer gaan opletten op mijn teamgenoten om zo goed te weten waar iedereen mee bezig is.**
-
-**specifiek:**
-
-ik heb in onze groep de taak als voorzitter alleen merk ik op dit moment dat ik niet goed weet waar iedereen mee bezig is. Dat is niet handig omdat ik dan als voorzitter niet goed kan inschatten of iedereen op schema ligt.
-
-**meetbaar:**
-
-Ik wil dat ik aan het einde van de blok meer contact heb gehad met mijn teamgenoten en dat ik weet waar iedereen mee bezig is. Dit zorgt ervoor dat ik beter kan inschatten of iedereen op schema ligt en of ik moet ingrijpen.
-
-**acceptabel:**
-
-Dit is acceptabel omdat ik als voorzitter de taak heb om te zorgen dat iedereen op schema ligt en dat iedereen weet wat hij/zij moet doen. Zonder deze informatie kan ik niet goed mijn taak uitvoeren waardoor de teamgenoten niet goed kunnen werken.
-
-**realistisch:**
-
-Dit is realistisch omdat ik als voorzitter de taak heb om te zorgen dat iedereen op schema ligt en dat iedereen weet wat hij/zij moet doen. 
-
-**tijdgebonden:**
-
-Ik wil dit doel behalen aan het einde van de blok. Dit is een realistische tijd omdat ik dan genoeg tijd heb om dit doel te behalen. Ik kan mij later ook nog tijdens de opleiding verbeteren op dit punt.
-
---
-
-2 **Ik wil in de volgende blok meer gaan opletten op mijn eigen documentatie.**
-
-**specifiek:**
-
-In tegen stelling tot vorig jaar hou ik nu mijn documentatie niet goed bij. Dit is niet handig omdat ik dan niet goed kan zien wat ik daadwerkelijk heb gedaan en wat ik allemaal heb geleerd. Ook is het fijn dat ik dan kan terug kijken op mijn werk als ik het later nodig heb. Ik wil daarom minimaal 2 keer per week mijn documentatie bijwerken.
-
-**meetbaar:**
-
-Ik wil dat ik aan het einde van de blok mijn documentatie goed heb bijgehouden. Dit zorgt ervoor dat ik kan zien wat ik heb gedaan en wat ik allemaal heb geleerd. Ook kan ik dan terug kijken op mijn werk als ik het later nodig heb.
-
-**acceptabel:**
-
-Dit is acceptabel omdat ik dan toekomst gericht kan werken. Ik kan dan terug kijken op mijn werk als ik het later nodig heb. Ook kan ik dan zien wat ik heb gedaan en wat ik allemaal heb geleerd.
-
-**realistisch:**
-
-Dit is realistisch omdat ik dit vorig jaar ook heb gedaan. Ik weet hoe ik mijn documentatie moet bijhouden en wat ik allemaal moet opschrijven. Mijn eis van 2 keer per week is ook realistisch omdat ik dan genoeg tijd heb om mijn documentatie bij te werken. 
-
-**tijdgebonden:**
-
-Ik wil dit doel behalen aan het einde van de blok. Dit is een realistische tijd omdat ik dan genoeg tijd heb om mijn documentatie bij te werken.
--- a/docs/Ishak/smartleerdoel.pdf
+++ b/docs/Ishak/smartleerdoel.pdf
--- a/docs/code/idea.md
+++ b/docs/code/idea.md
@@ -22,3 +22,32 @@ The ideas we came up with where:
 - A mobile scarecrow that will ride around a farm scaring birds, checking for optimal growing conditions using sensors, and checking (and possibly fighting) pests.
 - A robot that would be sent into buildings with a gasleak and check where the leak is the strongest and thus where it would most likely be coming from. (also possible lighting mechanic for controlled explosions in case of preventable disasters without loss of a human life)
 - A mobile fire extinguisher placed in buildings or terrains that are prone to fires. The robot would be able to detect fires and extinguish them before they get out of hand. Or control the fire before the fire department arrives.
+- A mobile solar panel that moves to the spot with the harshest sunlight (without a certain area). After which it would plug itself into a battery and charge it. This battery could then be used to power a building or a car.
+- A robot that is dropped in dangerous environments to scout the area and report back to the people who sent it. This could be used in warzones, dangerous buildings, or other dangerous environments.
+It scouts for stuff in the air using air sensors, it scouts for stuff on the ground using cameras, and it scouts for possible hazards like fire using infrared and other sensors.
+
+### Decision
+
+In the end we decided to go with the final idea. This was partially due to the  fact that it merges some of the most interresting aspect of the other ideas. We think that this is the most fitting with our goal oof minimizing the loss of human life because it can be used in not just fires or gasleaks but also in warzones and other dangerous environments. This makes it so that it has way more usecases than the other ideas and the buyer has enough with just these robots to cover a lot of dangerous situations.
+
+## Realization & Plan
+
+### Hardware
+
+#### Sensors
+
+placeholder
+
+#### IC's
+
+placeholder
+
+#### Motors
+
+placeholder
+
+#### Batteries
+
+placeholder
+
+### Software
--- a/docs/explanation/Sensorbehuizing.md
+++ b/docs/explanation/Sensorbehuizing.md
@@ -0,0 +1,28 @@
+# Sensor behuizing
+
+Voor de sensoren op onze Kobuki wouden wij graag een behuizing zodat deze sensoren niet los liggen op de Kobuki.
+
+Deze behuizing had een paar eisen en die eisen waren als volgt
+- Hij moet klein zijn zodat hij niet veel ruimte in neemt op de Kobuki.
+- De behuizing moet makkelijk vast te maken zijn aan de Kobuki.
+- In de behuizing moet een esp32 passen en de 3 sensoren.
+- De behuizing moet makkelijk uit elkaar te halen zijn zodat als we onderhoud moeten plegen dit makkelijk kan.
+
+Met deze eisen zijn we uiteindelijk een behuizing gaan maken in onshape.
+Onshape is gratis ontwerp software wat te gebruiken is via je browser.
+Hierdoor hoef je dus geen applicatie te runnen op je computer en kan je op elk apparaat inloggen om zo gemakkelijk door te gaan aan je ontwerp.
+Ik (Yannick) heb voor deze software gekozen omdat ik deze software al veel vaker heb gebruikt en hier dus al bekend mee ben.
+
+Uiteindelijk zijn we op het volgende design uitgekomen.
+![Afbeelding van de behuizing](images/Behuizingfoto1.png)
+![Afbeelding van de behuizing van bovenaf](images/Behuizingfoto2.png)
+
+Wij hebben gekozen voor dit design omdat dit de breedte en lengte heeft van een esp32 dus de esp past precies waardoor wij hem niet nog extra vast hoeven te zetten.
+Er zitten gaten in de zijkant van het bakje voor de kabel en voor een 5 volt kabel voor de MQ5 sensor.
+De dht11 sensor past er precies in en deze blijft daardoor precies vast zitten.
+Voor de M5stack sensor is er een gat gemaakt zodat deze kabel erdoorheen past en vervolgens wordt deze sensor op de bovenkant van de behuizing vastgeplakt.
+Voor de MQ5 sensor is een gat gemaakt waar de sensor door heen kan en het printplaatje wordt aan de onderkant vast gemaakt met stevige M3 tape.
+
+In de onderkant van de behuizing zitten 2 gaten hiermee kan de behuizing goed vastgemaakt worden aan de kobuki.
+De onderkant en de bovenkant van de behuizing zijn makkelijk uit elkaar te halen omdat deze doormiddel van 4 sterke magneten aan elkaar vast zitten.
+Hierdoor is het ook makkelijk om onderhoud te plegen omdat het bakje door de magneten makkelijk uit elkaar te halen is maar niet zo makkelijk dat hij door trillingen los kan komen.
--- a/docs/explanation/Sensoronderzoek.md
+++ b/docs/explanation/Sensoronderzoek.md
@@ -0,0 +1,79 @@
+# Sensor onderzoek
+
+In dit bestand gaan we onderzoek doen naar de sensoren die we willen gebruiken op de Kobuki.
+Hierin gaan we meerdere sensoren vergelijken met elkaar en kijken welke wij het beste kunnen gebruiken voor ons project.
+
+## Probleem
+Voor ons project moeten wij een manier vinden om gassen/stoffen te detecteren zodat je in een gebouw weet waar je niet veilig naartoe kan.
+
+## De vraag
+> Welke sensoren kunnen wij het beste gebruiken om schadelijke gassen/stoffen te vinden in een gebouw ?
+
+## Voorwaarden
+De voorwaarden waar de sensors aan moeten voldoen zijn:
+
+- De sensoren moeten op de kobuki passen.
+- We moeten zo weining mogelijk sensoren gebruiken zodat we genoeg plek over houden voor andere onderdelen van de Kobuki.
+
+## Hoe aansluiten
+Wij gaan deze sensoren aansluiten op een esp32 en deze laten wij via MQTT de gegevens doorsturen naar de raspberry pi.
+
+## De sensoren.
+Wij zijn uitgekomen op 3 sensoren.
+
+- De dht11 sensor
+- De tvoc/eC02 Gas Unit.
+- Gravity: Elektrochemische zuurstof-/O2-sensor (0-25%Vol, I2C) SEN0322
+- Gassensor MQ-5 module (OT2018-D55)
+
+Wij hebben voor de Dht11 en de tvoc/eC02 gas unit gekozen omdat wij deze bij james konden lenen. En wij hebben gekozen voor de Gassensor MQ-5 module (OT2018-D55) Omdat dit een mooie kleine sensor is die wij makkelijk kwijt kunnen op de kobuki.
+Voor de o2 sensor hebben wij gekozen voor de Gravity: Elektrochemische zuurstof-/O2-sensor (0-25%Vol, I2C) SEN0322. Deze sensor is helaas op het moment van schrijven (29-10-2024) niet op voorraad dus deze sensor kunnen wij helaas nog niet toevoegen. Zodra deze sensor op voorraad is zal ik deze bestellen en aan de esp toevoegen.
+
+## Sensor uitleg
+#### Dht11
+De dht11 is een eenvoudige en goedkope sensor die wordt gebruikt om de temperatuur en luchtvochtigheid te meten.
+
+De sensor bevat de volgende onderdelen om te werken:
+- Een thermistor : Dit is een component die temperatuur meet door variaties in elektronische weerstand.
+- Een capacitieve vochtigheidssensor : Deze meet de relatieve luchtvochtigheid door de verandering in het materiaal tussen de condensatorplaten te meten. Deze verandering gebeurt door de waterdamp in de lucht.
+- Een geïntegreerde microcontroller : Deze microcontroller verwerkt de gegevens van de sensoren en zet deze om in een digitaal signaal.
+
+Het meetbereik van de sensor is 0 tot 50 graden voor temperatuur en voor de luchtvochtigheid is het 20 tot 90%
+Hierbij is de temperatuur tot ±2 graden nauwkeurig en de luchtvochtigheid ±5 procent.
+
+De DHT11 kan aangesloten worden op een 3.3 of 5 volt voeding.
+Wij kunnen deze sensor dus zonder problemen compleet aansluiten op de esp32s3
+
+#### Tvoc/eC02 Gas Unit
+De M5Stack TVOC/eCO2-Gassensensor-eenheid (SGP30) is een compacte sensor ontwikkeld om vluchtige organische stoffen (TVOC) en schijnbare CO₂-concentraties (eCO2) te meten. 
+Deze component maakt gebruik van de SGP30-sensor van Adafruit
+
+De SGP30 is gebaseerd op een metal-oxide (MOX) halfgeleidertechnologie. 
+Deze technologie detecteert veranderingen in elektrische weerstand bij blootstelling aan vluchtige organische stoffen (VOC's), 
+zoals ethanol en aceton.
+
+De sensor bevat ingebouwde algoritmes om de gemeten VOC waarden om te zetten in tvoc en eco2.
+- TVOC : Dit is de totale concentratie vluchtige organische stoffen.
+- ECO2 : Dit is een geschatte koolstofdioxideconcentratie. 
+Dit is een schatting op basis van de VOC metingen.
+
+Deze sensor werkt op 3.3 en 5 volt dus ook voor deze sensor kunnen wij de esp32s3 gebruiken.
+
+#### MQ5 Gassensor
+
+De MQ5-gassensor is een veelgebruikte sensor voor het detecteren van brandbare gassen, zoals aardgas (methaan), vloeibaar petroleumgas (LPG), waterstof en koolmonoxide. Het werkt op basis van veranderingen in elektrische weerstand van het sensorelement wanneer het wordt blootgesteld aan specifieke gassen.
+
+De kern van de MQ5 sensor is een chemisch gecoat metalen oxide (meestal tinoxide). 
+Dit materiaal reageert op de aanwezigheid van brandbare gassen.
+Wanneer de sensor gasdeeltjes detecteert, reageren deze met zuurstofionen op het oppervlak van het tinoxide. Deze reactie veroorzaakt een verandering in de elektrische geleidbaarheid (weerstand) van het materiaal.
+De verandering in weerstand wordt door een elektronisch circuit omgezet in een elektrisch signaal dat de concentratie van gas vertegenwoordigt.
+
+De MQ5 sensor heeft ook een ingebouwde verwarmingsspiraal die het sensorelement op een hoge temperatuur houdt (ongeveer 300-500°C). Deze temperatuur zorgt ervoor dat gassen efficiënt reageren met het tinoxide-oppervlak.
+
+Het enige nadeel van deze sensor is dat hij niet selectief is dus de sensor kan geen onderscheid maken tussen bijvoorbeeld methaan en lpg.
+
+## Aansluitschema
+![Afbeelding aansluitschema](images/Aansluitschema_sensors.png)
+
+Hierboven is te zien hoe wij de sensoren hebben aangesloten op de esp32.
+
--- a/docs/explanation/image.png
+++ b/docs/explanation/image.png
--- a/docs/explanation/images/Aansluitschema_sensors.png
+++ b/docs/explanation/images/Aansluitschema_sensors.png
--- a/docs/explanation/images/Behuizingfoto1.png
+++ b/docs/explanation/images/Behuizingfoto1.png
--- a/docs/explanation/images/Behuizingfoto2.png
+++ b/docs/explanation/images/Behuizingfoto2.png
--- a/docs/explanation/sensoren.md
+++ b/docs/explanation/sensoren.md
@@ -0,0 +1,33 @@
+# Wat gaan we maken
+
+## Sensoren
+* Camera
+* GPS module
+* Temparatuur sensor
+* TVOC sensor
+* ECO2 sensor
+* LDR sensor
+* Time of Flight sensor
+
+## Wat gaan we met de sensoren doen?
+
+### Camera
+De camera word gebruikt om foto's te maken in de omgeving in het geval van informatie verkrijgen voor als de robot bijvoorbeeld vast zit, geeft ook optie om informatie te krijgen zonder op de plek zelf te zijn.
+
+### GPS module
+De GPS module word gebruikt om de locatie van de robot te bepalen en aan te geven waar bijzonderheden bevinden.
+
+### Temparatuur, TVOC en ECO2 sensor
+Deze sensoren zijn bedoeld om de omgeving te meten en te kijken of de omgeving veilig is voor mensen om in te gaan.
+
+### LDR sensor
+De LDR sensor word gebruikt om de lichtsterkte te meten en te kijken of er een lamp op de robot aan moet gaan voor de camera.
+
+### Time of Flight sensor
+De Time of Flight sensor word gebruikt om de afstand te meten tussen de robot en de muur, zodat de robot niet tegen de muur aan botst.
+
+## Het project
+Bij brand of op fabrieksterreinen met gevaarlijke stoffen kan het nodig zijn om een verkenning te 
+doen van een verdachte omgeving. Het is dan niet verstandig om mensen naar binnen te sturen, in 
+die gevallen vallen de hulpdiensten terug om een verkenningsrobot. Het doel van het project is het 
+realiseren van een verkenningsrobot die zelfstandig een gevaarlijke omgeving in kaart kan brengen.
--- a/src/Arduino/Sensors/Sensors.ino
+++ b/src/Arduino/Sensors/Sensors.ino
@@ -0,0 +1,123 @@
+#include <DHT.h>
+#include <Wire.h>
+#include "Adafruit_SGP30.h"
+#include <WiFi.h>
+#include <PubSubClient.h>
+#include <ArduinoWebsockets.h>
+
+using namespace websockets;
+
+Adafruit_SGP30 sgp;
+
+// Definieert de pins voor de sensoren
+#define DHTPIN 4
+#define DHTTYPE DHT11
+
+#define MQ5_PIN 2
+
+#define SDA_PIN 10
+#define SCL_PIN 11
+
+DHT dht(DHTPIN, DHTTYPE);
+
+// WiFi en MQTT instellingen
+const char* ssid = "";
+const char* password = "";
+
+const char* mqtt_server = "192.168.68.104";
+const int mqtt_port = 8080; //websocket-poort
+const char* mqtt_topic = "sensors/data";
+
+// MQTT client
+WiFiClient espClient;
+WebsocketsClient websocket;
+PubSubClient client(espClient);
+
+// Functie om verbinding te maken met WiFi
+void setup_wifi() {
+  Serial.print("Verbinden met WiFi...");
+  WiFi.begin(ssid, password);
+  while (WiFi.status() != WL_CONNECTED) {
+    delay(500);
+    Serial.print(".");
+  }
+  Serial.println("Verbonden!");
+}
+
+void reconnectMQTT() {
+  while (!client.connected()) {
+    Serial.print("Verbinding maken met MQTT via WebSockets...");
+    if (client.connect("ESP32Client")) {
+      Serial.println("Verbonden!");
+    } else {
+      Serial.print("Fout: ");
+      Serial.print(client.state());
+      delay(5000);
+    }
+  }
+}
+
+void setup() {
+  // Start de seriële monitor
+  Serial.begin(9600);
+  
+  dht.begin();
+  
+   pinMode(MQ5_PIN, INPUT);
+ 
+   Wire.begin(SDA_PIN, SCL_PIN);
+  Serial.println("SGP30 test");
+
+  // SGP30 initialiseren
+  if (!sgp.begin()) {
+    Serial.println("SGP30 sensor niet gevonden :(");
+    while (1);
+  }
+  if (!sgp.IAQinit()) {
+    Serial.println("SGP30 IAQ-initialisatie mislukt!");
+    while (1);
+  }
+
+  // Verbind met WiFi en MQTT-broker
+  setup_wifi();
+  // Stel MQTT-broker in met websockets
+  client.setServer(mqtt_server, mqtt_port);
+  reconnectMQTT();
+}
+
+void loop() {
+  // Zorgt ervoor dat MQTT verbonden blijft
+  if (!client.connected()) {
+    reconnectMQTT();
+  }
+  client.loop();
+
+  float h = dht.readHumidity();
+  float t = dht.readTemperature();
+  int mq5Value = analogRead(MQ5_PIN);
+
+   // Check of de sensorwaarden geldig zijn
+   if (isnan(h) || isnan(t) || mq5Value < 0) {
+     Serial.println("Fout bij het lezen van de sensors!");
+     return;
+   }
+
+   // Maak een JSON-payload
+  String payload = "{";
+  payload += "\"humidity\":" + String(h) + ",";
+  payload += "\"temperature\":" + String(t) + ",";
+  payload += "\"mq5\":" + String(mq5Value) + ",";
+  payload += "\"tvoc\":" + String(sgp.TVOC) + ",";
+  payload += "\"eco2\":" + String(sgp.eCO2);
+  payload += "}";
+
+  // Verzend de payload via MQTT
+  if (client.publish(mqtt_topic, payload.c_str())) {
+    Serial.println("Bericht verzonden: " + payload);
+  } else {
+    Serial.println("Fout bij verzenden van bericht!");
+  }
+
+  // Wacht 5 seconden voor de volgende meting
+  delay(5000);
+}
--- a/src/Arduino/TestM5/TestM5.ino
+++ b/src/Arduino/TestM5/TestM5.ino
@@ -0,0 +1,40 @@
+// Test code is merged to main sensor code os this file is not needed anymore
+
+#include <Wire.h>
+#include "Adafruit_SGP30.h"
+
+Adafruit_SGP30 sgp;
+
+#define SDA_PIN 10
+#define SCL_PIN 11
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) { delay(10); }
+
+  Wire.begin(SDA_PIN, SCL_PIN);
+
+  Serial.println("SGP30 test");
+
+  if (!sgp.begin()) {
+    Serial.println("SGP30 sensor not found :(");
+    while (1);
+  }
+
+  // Start measurements (initialize baseline)
+  if (! sgp.IAQinit()) {
+    Serial.println("SGP30 IAQinit failed!");
+    while (1);
+  }
+}
+
+void loop() {
+  if (! sgp.IAQmeasure()) {
+    Serial.println("Measurement failed");
+    return;
+  }
+  Serial.print("TVOC "); Serial.print(sgp.TVOC); Serial.print(" ppb\t");
+  Serial.print("eCO2 "); Serial.print(sgp.eCO2); Serial.println(" ppm");
+
+  delay(1000); // 1 second delay
+}
--- a/src/C++/Driver/CMakeLists.txt
+++ b/src/C++/Driver/CMakeLists.txt
@@ -1,23 +1,12 @@
 cmake_minimum_required(VERSION 3.9)
 project(kobuki_control)
 set(CMAKE_CXX_STANDARD 23)
-
-# Find the Paho MQTT C++ library (static)
-find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
-find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
-
-include_directories(/usr/local/include)
-
 set(SOURCE_FILES
-    src/KobukiDriver/KobukiParser.cpp
-    src/KobukiDriver/KobukiParser.h
-    src/KobukiDriver/CKobuki.cpp
-    src/KobukiDriver/CKobuki.h
-    src/MQTT/MqttClient.cpp
-    src/MQTT/MqttClient.h
-    src/main.cpp)
+        src/KobukiParser.cpp
+        src/KobukiParser.h
+        src/CKobuki.cpp
+        src/CKobuki.h
+        src/main.cpp)

 add_executable(kobuki_control ${SOURCE_FILES})
-
-# Link the static libraries
-target_link_libraries(kobuki_control ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY} pthread)
+#target_link_libraries(kobuki_control )
--- a/src/C++/Driver/src/CKobuki.cpp
+++ b/src/C++/Driver/src/CKobuki.cpp
@@ -0,0 +1,683 @@
+#include "CKobuki.h"
+#include "errno.h"
+#include "termios.h"
+#include <cstddef>
+#include <iostream>
+#include <thread>
+
+
+// plot p;
+static std::vector<float> vectorX;
+static std::vector<float> vectorY;
+static std::vector<float> vectorGyroTheta;
+
+// obsluha tty pod unixom
+int set_interface_attribs2(int fd, int speed, int parity)
+{
+    struct termios tty;
+    memset(&tty, 0, sizeof tty);
+    if (tcgetattr(fd, &tty) != 0)
+    {
+        printf("error %d from tcgetattr", errno);
+        return -1;
+    }
+
+    cfsetospeed(&tty, speed);
+    cfsetispeed(&tty, speed);
+
+    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
+    // disable IGNBRK for mismatched speed tests; otherwise receive break
+    // as \000 chars
+    // tty.c_iflag &= ~IGNBRK;         // disable break processing
+    tty.c_lflag = 0; // no signaling chars, no echo,
+    // no canonical processing
+    tty.c_oflag = 0;     // no remapping, no delays
+    tty.c_cc[VMIN] = 0;  // read doesn't block
+    tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
+
+    tty.c_iflag &= ~(IGNBRK | INLCR | ICRNL | IXON | IXOFF |
+                     IXANY); // shut off xon/xoff ctrl
+
+    tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls,
+    // enable reading
+    tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
+    tty.c_cflag |= parity;
+    tty.c_cflag &= ~CSTOPB;
+    tty.c_cflag &= ~CRTSCTS;
+
+    if (tcsetattr(fd, TCSANOW, &tty) != 0)
+    {
+        printf("error %d from tcsetattr", errno);
+        return -1;
+    }
+    return 0;
+}
+
+void set_blocking2(int fd, int should_block)
+{
+    struct termios tty;
+    memset(&tty, 0, sizeof tty);
+    if (tcgetattr(fd, &tty) != 0)
+    {
+        printf("error %d from tggetattr", errno);
+        return;
+    }
+
+    tty.c_cc[VMIN] = should_block ? 1 : 0;
+    tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
+
+    if (tcsetattr(fd, TCSANOW, &tty) != 0)
+        printf("error %d setting term attributes", errno);
+}
+
+int CKobuki::connect(char *comportT)
+{
+    HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+    if (HCom == -1)
+    {
+        printf("Kobuki nepripojeny\n");
+        return HCom;
+    }
+    else
+    {
+        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");
+        return HCom;
+    }
+}
+
+unsigned char *CKobuki::readKobukiMessage()
+{
+    unsigned char buffer[1];
+    ssize_t Pocet;
+    buffer[0] = 0;
+    unsigned char *null_buffer(0);
+    // citame kym nezachytime zaciatok spravy
+    do
+    {
+        Pocet = read(HCom, buffer, 1);
+    } while (buffer[0] != 0xAA);
+    // mame zaciatok spravy (asi)
+    if (Pocet == 1 && buffer[0] == 0xAA)
+    {
+        // citame dalsi byte
+        do
+        {
+
+            Pocet = read(HCom, buffer, 1);
+
+        } while (Pocet != 1); // na linuxe -1 na windowse 0
+
+        // a ak je to druhy byte hlavicky
+        if (Pocet == 1 && buffer[0] == 0x55)
+        {
+            // precitame dlzku
+            Pocet = read(HCom, buffer, 1);
+
+            // ReadFile(hCom, buffer, 1, &Pocet, NULL);
+            if (Pocet == 1)
+            {
+                // mame dlzku.. nastavime vektor a precitame ho cely
+                int readLenght = buffer[0];
+                unsigned char *outputBuffer =
+                    (unsigned char *)calloc(readLenght + 4, sizeof(char));
+                outputBuffer[0] = buffer[0];
+                int pct = 0;
+
+                do
+                {
+                    Pocet = 0;
+                    int readpoc = (readLenght + 1 - pct);
+                    Pocet = read(HCom, outputBuffer + 1 + pct, readpoc);
+
+                    pct = pct + (Pocet == -1 ? 0 : Pocet);
+                } while (pct != (readLenght + 1));
+
+                // tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho
+                // for(int i=0;i<outputBuffer[0]+2;i++)
+                // {
+                // 	printf("%x ",outputBuffer[i]);
+                // }
+
+                return outputBuffer;
+            }
+        }
+    }
+
+    return null_buffer;
+}
+
+void CKobuki::setLed(int led1, int led2)
+{
+    unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00};
+    message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6];
+    uint32_t pocet;
+    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...
+void CKobuki::setPower(int value)
+{
+    if (value == 1)
+    {
+        unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF};
+        uint32_t pocet;
+        pocet = write(HCom, &message, 8);
+    }
+}
+
+void CKobuki::setTranslationSpeed(int mmpersec)
+{
+    unsigned char message[14] = {0xaa, 0x55, 0x0A, 0x0c, 0x02,
+                                 0xf0, 0x00, 0x01, 0x04, mmpersec % 256,
+                                 mmpersec >> 8, 0x00, 0x00, 0x00};
+    message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
+                  message[7] ^ message[8] ^ message[9] ^ message[10] ^
+                  message[11] ^ message[12];
+
+    uint32_t pocet;
+    pocet = write(HCom, &message, 14);
+}
+
+void CKobuki::setRotationSpeed(double radpersec)
+{
+    int speedvalue = radpersec * 230.0f / 2.0f;
+    unsigned char message[14] = {0xaa,
+                                 0x55,
+                                 0x0A,
+                                 0x0c,
+                                 0x02,
+                                 0xf0,
+                                 0x00,
+                                 0x01,
+                                 0x04,
+                                 speedvalue % 256,
+                                 speedvalue >> 8,
+                                 0x01,
+                                 0x00,
+                                 0x00};
+    message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
+                  message[7] ^ message[8] ^ message[9] ^ message[10] ^
+                  message[11] ^ message[12];
+
+    uint32_t pocet;
+    pocet = write(HCom, &message, 14);
+}
+
+void CKobuki::setArcSpeed(int mmpersec, int radius)
+{
+    if (radius == 0)
+    {
+        setTranslationSpeed(mmpersec);
+        return;
+    }
+
+    int speedvalue =
+        mmpersec * ((radius + (radius > 0 ? 230 : -230)) / 2) / radius;
+    unsigned char message[14] = {0xaa,
+                                 0x55,
+                                 0x0A,
+                                 0x0c,
+                                 0x02,
+                                 0xf0,
+                                 0x00,
+                                 0x01,
+                                 0x04,
+                                 speedvalue % 256,
+                                 speedvalue >> 8,
+                                 radius % 256,
+                                 radius >> 8,
+                                 0x00};
+    message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
+                  message[7] ^ message[8] ^ message[9] ^ message[10] ^
+                  message[11] ^ message[12];
+    uint32_t pocet;
+    pocet = write(HCom, &message, 14);
+}
+
+void CKobuki::setSound(int noteinHz, int duration)
+{
+    int notevalue = floor((double)1.0 / ((double)noteinHz * 0.00000275) + 0.5);
+    unsigned char message[9] = {0xaa, 0x55, 0x05,
+                                0x03, 0x03, notevalue % 256,
+                                notevalue >> 8, duration % 256, 0x00};
+    message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
+                 message[7];
+
+    uint32_t pocet;
+    pocet = write(HCom, &message, 9);
+}
+
+void CKobuki::startCommunication(char *portname, bool CommandsEnabled, void *userDataL)
+{
+    connect(portname);
+    enableCommands(CommandsEnabled);
+    userData = userDataL;
+
+    int pthread_result;
+    pthread_result = pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this);
+    if (pthread_result != 0) {
+        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;
+        }
+        int ok = parser.parseKobukiMessage(parser.data, message);
+
+        // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
+        // citat
+        if (ok == 0)
+        {
+            loop(userData, parser.data);
+        }
+        free(message);
+    }
+    return 0;
+}
+
+long double CKobuki::gyroToRad(signed short GyroAngle)
+{
+
+    long double rad;
+    if (GyroAngle < 0)
+    {
+        rad = GyroAngle + 360;
+    }
+    else
+    {
+        rad = GyroAngle;
+    }
+    return (long double)rad * PI / 180.0;
+}
+
+long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data)
+{
+    if (iterationCount == 0)
+    {
+        prevLeftEncoder = Kobuki_data.EncoderLeft;
+        prevRightEncoder = Kobuki_data.EncoderRight;
+        prevTimestamp = Kobuki_data.timestamp;
+        prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
+        iterationCount++;
+    }
+
+    int dLeft;
+    if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000)
+    {
+        dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder +
+                (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536);
+    }
+    else
+    {
+        dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder;
+    }
+
+    int dRight;
+    if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000)
+    {
+        dRight = Kobuki_data.EncoderRight - prevRightEncoder +
+                 (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536);
+    }
+    else
+    {
+        dRight = Kobuki_data.EncoderRight - prevRightEncoder;
+    }
+
+    long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle);
+
+    if (dGyroTheta > PI)
+    {
+        dGyroTheta -= 2 * PI;
+    }
+    if (dGyroTheta < -1 * PI)
+    {
+        dGyroTheta += 2 * PI;
+    }
+
+    gyroTheta += dGyroTheta;
+
+    uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp;
+
+    long double mLeft = dLeft * tickToMeter;
+    long double mRight = dRight * tickToMeter;
+
+    if (mLeft == mRight)
+    {
+        x = x + mRight;
+    }
+    else
+    {
+        x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
+                    (sin((mRight - mLeft) / b + theta) - sin(theta));
+        y = y + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
+                    (cos((mRight - mLeft) / b + theta) - cos(theta));
+        theta = (mRight - mLeft) / b + theta;
+    }
+
+    displacement = (mRight + mLeft) / 2;
+    integratedGyroTheta = integratedGyroTheta + dGyroTheta;
+    gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2);
+
+    gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2);
+
+    totalLeft += dLeft;
+    totalRight += dRight;
+
+    // ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo?
+    directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1);
+    directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1);
+    dTimestamp = (Kobuki_data.timestamp < prevTimestamp
+                      ? prevTimestamp - Kobuki_data.timestamp + 65536
+                      : dTimestamp);
+
+    prevLeftEncoder = Kobuki_data.EncoderLeft;
+    prevRightEncoder = Kobuki_data.EncoderRight;
+    prevTimestamp = Kobuki_data.timestamp;
+    prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
+
+    //    std::cout << "X: " << x
+    //              << " Y: " << y
+    //              << " Theta: " << theta
+    //              << "Gyro theta:" << gyroTheta
+    //              << std::endl;
+
+    static long counter = 0;
+    
+    //vector for data plotting
+    vectorX.push_back(gx);
+    vectorY.push_back(gy);
+    vectorGyroTheta.push_back(gyroTheta);
+
+    // if (counter % 100 == 0) {
+    //     p.plot_data(vectorY, vectorX);
+    // }
+    counter++;
+
+    return 0;
+}
+
+
+
+
+// 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
+void CKobuki::goStraight(long double distance){
+    long double u_translation = 0; // controlled magnitude, speed of the robot in motion
+    long double w_translation = distance; // requested value
+
+  // controller parameters
+  long double Kp_translation = 4500;
+  long double e_translation = 0;
+  int upper_thresh_translation = 600;
+  int lower_thresh_translation = 40;
+  int translation_start_gain = 20;
+
+    long double u_rotation = 0; // controlled magnitude
+    long double w_rotation = 0;
+    long double Kp_rotation = 57;
+    long double e_rotation = 0;
+
+    x = 0;
+    y = 0;
+    theta = 0;
+
+    long i = 5;
+    //send command and hold until robot reaches point
+    while (fabs(x - w_translation) > 0.005 && x < w_translation)
+    {
+        e_translation = w_translation - x;
+        u_translation = Kp_translation * e_translation;
+
+        e_rotation = w_rotation - theta;
+        if (!e_rotation == 0)
+            u_rotation = Kp_rotation / e_rotation;
+
+        // limit translation speed
+        if (u_translation > upper_thresh_translation)
+            u_translation = upper_thresh_translation;
+        if (u_translation < lower_thresh_translation)
+            u_translation = lower_thresh_translation;
+
+        // rewrite starting speed with line
+        if (i < u_translation)
+        {
+            u_translation = i;
+        }
+
+        if (fabs(u_rotation) > 32767)
+        {
+            u_rotation = -32767;
+        }
+
+        if (u_rotation == 0)
+        {
+            u_rotation = -32767;
+        }
+        //send command to robot
+        this->setArcSpeed(u_translation, u_rotation);
+
+        // increment starting speed
+        i = i + translation_start_gain;
+    }
+    this->setTranslationSpeed(0);
+}
+
+/// the method performs the rotation, it rotates using the regulator, the
+/// gyroscope serves as feedback, because it is much more accurate than encoders
+void CKobuki::doRotation(long double th)
+{
+    long double u = 0;  // controlled variable, angular speed of the robot during movement
+    long double w = th; // desired value in radians
+    long double Kp = PI;
+    long double e = 0;
+    int thresh = PI / 2;
+
+    theta = 0;
+    x = 0;
+    y = 0;
+    gyroTheta = 0;
+
+    long double i = 0;
+
+    if (w > 0)
+    {
+        while (gyroTheta < w)
+        {
+            e = w - gyroTheta;
+            u = Kp * e;
+
+            if (u > thresh)
+                u = thresh;
+            if (u < 0.4)
+                u = 0.4;
+
+            if (i < u)
+            {
+                u = i;
+            }
+
+            std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
+            this->setRotationSpeed(-1 * u);
+            usleep(25 * 1000);
+            i = i + 0.1;
+        }
+    }
+    else
+    {
+        while (gyroTheta > w)
+        {
+            e = w - gyroTheta;
+            u = Kp * e * -1;
+
+            if (u > thresh)
+                u = thresh;
+            if (u < 0.4)
+                u = 0.4;
+
+            if (i < u)
+            {
+                u = i;
+            }
+
+            std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
+            this->setRotationSpeed(u);
+            usleep(25 * 1000);
+            i = i + 0.1;
+        }
+    }
+
+    std::cout << "Rotation done" << std::endl;
+    this->setRotationSpeed(0);
+    usleep(25*1000);
+}
+
+// combines navigation to a coordinate and rotation by an angle, performs movement to
+// the selected coordinate in the robot's coordinate system
+void CKobuki::goToXy(long double xx, long double yy)
+{
+    long double th;
+
+    yy = yy * -1;
+
+    th = atan2(yy, xx);
+    doRotation(th);
+
+    long double s = sqrt(pow(xx, 2) + pow(yy, 2));
+
+    // resetnem suradnicovu sustavu robota
+    x = 0;
+    y = 0;
+    iterationCount = 0;
+    theta = 0;
+
+    // std::cout << "mam prejst: " << s << "[m]" << std::endl;
+
+    goStraight(s);
+
+    usleep(25 * 1000);
+    return;
+}
+
+/// @brief Makes the Kobuki go forward
+/// @param speedvalue speed of robot in mm/s
+/// @param distance   distance in meters
+void CKobuki::forward(int speedvalue) {
+  // Use the goStraight logic to determine the speed and distance
+
+  // Calculate the actual speed and radius values based on the conversion table
+  int actual_speed = speedvalue;
+  int actual_radius = 0; // Pure translation (straight line)
+
+unsigned char message[11] = {
+    0xaa,                // Start byte 1
+    0x55,                // Start byte 2
+    0x08,                // Payload length (the first 2 bytes dont count)
+    0x01,                // payload type (0x01 = control command)
+    0x04,                // Control byte or additional identifier
+    actual_speed % 256,  // Lower byte of speed value
+    actual_speed >> 8,   // Upper byte of speed value
+    0x00,                // Placeholder for radius
+    0x00,                // Placeholder for radius
+    0x00                 // Placeholder for checksum
+};
+
+// Calculate checksum
+message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
+              message[7] ^ message[8] ^ message[9];
+
+  // Send the message
+  uint32_t pocet;
+  pocet = write(HCom, &message, 11);
+  pocet = write(HCom, &message, 11);
+
+}
+
+/// @brief Makes the kobuki rotate
+/// @param degrees Rotation in degrees
+void CKobuki::Rotate(int degrees) {
+
+    // convert raidans to degrees
+    float radians = degrees * PI / 180.0;
+
+    // Calculate the rotation speed in radians per second
+    double radpersec = 1; 
+
+    // calculator rotation time and give absolute value
+    float rotation_time = std::abs(radians / radpersec);
+    
+    // Use original function to set the rotation speed in mm/s
+    setRotationSpeed(radians);
+
+    // Sleep for the calculated rotation time
+    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(rotation_time * 1000)));
+
+    // Stop the robot after the rotation
+    setRotationSpeed(0);
+}
+
+
+void CKobuki::robotSafety() {
+    while (true) {
+
+        if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight || 
+            parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight) {
+            std::cout << "Safety condition triggered!" << std::endl; // Debug print
+            forward(-100); // reverse the robot
+
+        }
+
+    }
+}
+
+void CKobuki::sendNullMessage(){
+
+    unsigned char message[11] = {
+    0xaa,                // Start byte 1
+    0x55,                // Start byte 2
+    0x08,                // Payload length (the first 2 bytes dont count)
+    0x01,                // payload type (0x01 = control command)
+    0x04,                // Control byte or additional identifier
+    0x00,  // Lower byte of speed value
+    0x00,   // Upper byte of speed value
+    0x00,                // Placeholder for radius
+    0x00,                // Placeholder for radius
+    0x00                 // Placeholder for checksum
+};
+    
+
+message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
+              message[7] ^ message[8] ^ message[9];
+
+  // Send the message
+  uint32_t pocet;
+  pocet = write(HCom, &message, 11);
+}
--- a/src/C++/Driver/src/KobukiDriver/CKobuki.h
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.h
@@ -12,7 +12,7 @@
 ////*************************************************************************************
 #ifndef KOBUKI_CLASS_123456789
 #define KOBUKI_CLASS_123456789
-#define PI          3.141592653589793238462643383279502884L /* pi */
+#define PI  3.141592653589793238462643383279502884L /* pi */
 #define MS_INSTRUCTION_DELAY 25

 #include <stdio.h>
@@ -35,91 +35,6 @@

 using namespace std;

-
-// typedef struct
-// {
-	
-// 	unsigned short x;
-// 	unsigned short y;
-// 	unsigned short z;
-
-// }TRawGyroData;
-// typedef struct
-// {
-// 	//Hardware Version
-// 	unsigned char HardwareVersionMajor;
-// 	unsigned char HardwareVersionMinor;
-// 	unsigned char HardwareVersionPatch;
-// 	//Firmware Version
-// 	unsigned char FirmwareVersionMajor;
-// 	unsigned char FirmwareVersionMinor;
-// 	unsigned char FirmwareVersionPatch;
-
-// 	//Unique Device IDentifier(UDID)
-// 	unsigned int UDID0;
-// 	unsigned int UDID1;
-// 	unsigned int UDID2;
-// 	//Controller Info
-// 	unsigned char PIDtype;
-// 	unsigned int PIDgainP;
-// 	unsigned int PIDgainI;
-// 	unsigned int PIDgainD;
-// }TExtraRequestData;
-
-// typedef struct
-// {
-// 	//---basic package
-// 	unsigned short timestamp;
-// 	//bumpers
-// 	bool BumperLeft;
-// 	bool BumperCenter;
-// 	bool BumperRight;
-// 	//cliff
-// 	bool CliffLeft;
-// 	bool CliffCenter;
-// 	bool CliffRight;
-// 	// wheel drop
-// 	bool WheelDropLeft;  
-// 	bool WheelDropRight; 
-// 	//wheel rotation
-// 	unsigned short EncoderRight;
-// 	unsigned short EncoderLeft;
-// 	unsigned char PWMright;
-// 	unsigned char PWMleft;
-// 	//buttons
-// 	unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three)
-// 	//power
-// 	unsigned char Charger;
-// 	unsigned char Battery;
-// 	unsigned char overCurrent;
-// 	//---docking ir
-// 	unsigned char IRSensorRight;
-// 	unsigned char IRSensorCenter;
-// 	unsigned char IRSensorLeft;
-// 	//---Inertial Sensor Data
-// 	signed short GyroAngle;
-// 	unsigned short GyroAngleRate;
-// 	//---Cliff Sensor Data
-// 	unsigned short CliffSensorRight;
-// 	unsigned short CliffSensorCenter;
-// 	unsigned short CliffSensorLeft;
-// 	//---Current
-// 	unsigned char wheelCurrentLeft;
-// 	unsigned char wheelCurrentRight;
-// 	//---Raw Data Of 3D Gyro
-// 	unsigned char frameId;
-// 	std::vector<TRawGyroData> gyroData;
-// 	//---General Purpose Input
-// 	unsigned short digitalInput;
-// 	unsigned short analogInputCh0;
-// 	unsigned short analogInputCh1;
-// 	unsigned short analogInputCh2;
-// 	unsigned short analogInputCh3;
-// 	//---structure with data that appears only on request
-// 	TExtraRequestData extraInfo;
-// }TKobukiData;
-
-
 typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data);

 class CKobuki
@@ -156,10 +71,13 @@ public:

 	// control functions 
 	void goStraight(long double distance);
-	void forward(int speedvalue, long double distance);
+	void forward(int speedvalue);
 	void doRotation(long double th);
 	void goToXy(long double xx, long double yy);
+	void Rotate(int degrees);
 	std::ofstream odometry_log;
+	void robotSafety();
+	void sendNullMessage();
 	KobukiParser parser;


--- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
@@ -1,757 +0,0 @@
-#include "CKobuki.h"
-#include "errno.h"
-#include "termios.h"
-#include <cstddef>
-#include <iostream>
-
-// plot p;
-static std::vector<float> vectorX;
-static std::vector<float> vectorY;
-static std::vector<float> vectorGyroTheta;
-
-// obsluha tty pod unixom
-int set_interface_attribs2(int fd, int speed, int parity) {
-  struct termios tty;
-  memset(&tty, 0, sizeof tty);
-  if (tcgetattr(fd, &tty) != 0) {
-    printf("error %d from tcgetattr", errno);
-    return -1;
-  }
-
-  cfsetospeed(&tty, speed);
-  cfsetispeed(&tty, speed);
-
-  tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
-  // disable IGNBRK for mismatched speed tests; otherwise receive break
-  // as \000 chars
-  // tty.c_iflag &= ~IGNBRK;         // disable break processing
-  tty.c_lflag = 0; // no signaling chars, no echo,
-  // no canonical processing
-  tty.c_oflag = 0;     // no remapping, no delays
-  tty.c_cc[VMIN] = 0;  // read doesn't block
-  tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
-
-  tty.c_iflag &= ~(IGNBRK | INLCR | ICRNL | IXON | IXOFF |
-                   IXANY); // shut off xon/xoff ctrl
-
-  tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls,
-  // enable reading
-  tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
-  tty.c_cflag |= parity;
-  tty.c_cflag &= ~CSTOPB;
-  tty.c_cflag &= ~CRTSCTS;
-
-  if (tcsetattr(fd, TCSANOW, &tty) != 0) {
-    printf("error %d from tcsetattr", errno);
-    return -1;
-  }
-  return 0;
-}
-
-void set_blocking2(int fd, int should_block) {
-  struct termios tty;
-  memset(&tty, 0, sizeof tty);
-  if (tcgetattr(fd, &tty) != 0) {
-    printf("error %d from tggetattr", errno);
-    return;
-  }
-
-  tty.c_cc[VMIN] = should_block ? 1 : 0;
-  tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
-
-  if (tcsetattr(fd, TCSANOW, &tty) != 0)
-    printf("error %d setting term attributes", errno);
-}
-
-int CKobuki::connect(char *comportT) {
-  HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
-
-  if (HCom == -1) {
-    printf("Kobuki nepripojeny\n");
-    return HCom;
-  } else {
-    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");
-    return HCom;
-  }
-}
-
-unsigned char *CKobuki::readKobukiMessage() {
-  unsigned char buffer[1];
-  ssize_t Pocet;
-  buffer[0] = 0;
-  unsigned char *null_buffer(0);
-  // citame kym nezachytime zaciatok spravy
-  do {
-    Pocet = read(HCom, buffer, 1);
-  } while (buffer[0] != 0xAA);
-  // mame zaciatok spravy (asi)
-  if (Pocet == 1 && buffer[0] == 0xAA) {
-    // citame dalsi byte
-    do {
-
-      Pocet = read(HCom, buffer, 1);
-
-    } while (Pocet != 1); // na linuxe -1 na windowse 0
-
-    // a ak je to druhy byte hlavicky
-    if (Pocet == 1 && buffer[0] == 0x55) {
-      // precitame dlzku
-      Pocet = read(HCom, buffer, 1);
-
-      // ReadFile(hCom, buffer, 1, &Pocet, NULL);
-      if (Pocet == 1) {
-        // mame dlzku.. nastavime vektor a precitame ho cely
-        int readLenght = buffer[0];
-        unsigned char *outputBuffer =
-            (unsigned char *)calloc(readLenght + 4, sizeof(char));
-        outputBuffer[0] = buffer[0];
-        int pct = 0;
-
-        do {
-          Pocet = 0;
-          int readpoc = (readLenght + 1 - pct);
-          Pocet = read(HCom, outputBuffer + 1 + pct, readpoc);
-
-          pct = pct + (Pocet == -1 ? 0 : Pocet);
-        } while (pct != (readLenght + 1));
-
-        // tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho
-        // for(int i=0;i<outputBuffer[0]+2;i++)
-        // {
-        // 	printf("%x ",outputBuffer[i]);
-        // }
-
-        return outputBuffer;
-      }
-    }
-  }
-
-  return null_buffer;
-}
-
-void CKobuki::setLed(int led1, int led2) {
-  unsigned char message[8] = {0xaa,
-                              0x55,
-                              0x04,
-                              0x0c,
-                              0x02,
-                              0x00,
-                              (unsigned char)((led1 + led2 * 4) % 256),
-                              0x00};
-  message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6];
-  uint32_t pocet;
-  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...
-void CKobuki::setPower(int value) {
-  if (value == 1) {
-    unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF};
-    uint32_t pocet;
-    pocet = write(HCom, &message, 8);
-  }
-}
-
-void CKobuki::setTranslationSpeed(int mmpersec) {
-  unsigned char message[14] = {0xaa,          0x55, 0x0A, 0x0c, 0x02,
-                               0xf0,          0x00, 0x01, 0x04, mmpersec % 256,
-                               mmpersec >> 8, 0x00, 0x00, 0x00};
-  message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
-                message[7] ^ message[8] ^ message[9] ^ message[10] ^
-                message[11] ^ message[12];
-
-  uint32_t pocet;
-  pocet = write(HCom, &message, 14);
-}
-
-void CKobuki::setRotationSpeed(double radpersec) {
-  int speedvalue = radpersec * 230.0f / 2.0f;
-  unsigned char message[14] = {0xaa,
-                               0x55,
-                               0x0A,
-                               0x0c,
-                               0x02,
-                               0xf0,
-                               0x00,
-                               0x01,
-                               0x04,
-                               speedvalue % 256,
-                               speedvalue >> 8,
-                               0x01,
-                               0x00,
-                               0x00};
-  message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
-                message[7] ^ message[8] ^ message[9] ^ message[10] ^
-                message[11] ^ message[12];
-
-  uint32_t pocet;
-  pocet = write(HCom, &message, 14);
-}
-
-void CKobuki::setArcSpeed(int mmpersec, int radius) {
-  if (radius == 0) {
-    setTranslationSpeed(mmpersec);
-    return;
-  }
-
-  int speedvalue =
-      mmpersec * ((radius + (radius > 0 ? 230 : -230)) / 2) / radius;
-  unsigned char message[14] = {0xaa,
-                               0x55,
-                               0x0A,
-                               0x0c,
-                               0x02,
-                               0xf0,
-                               0x00,
-                               0x01,
-                               0x04,
-                               speedvalue % 256,
-                               speedvalue >> 8,
-                               radius % 256,
-                               radius >> 8,
-                               0x00};
-  message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
-                message[7] ^ message[8] ^ message[9] ^ message[10] ^
-                message[11] ^ message[12];
-  uint32_t pocet;
-  pocet = write(HCom, &message, 14);
-}
-
-void CKobuki::setSound(int noteinHz, int duration) {
-  int notevalue = floor((double)1.0 / ((double)noteinHz * 0.00000275) + 0.5);
-  unsigned char message[9] = {0xaa,           0x55,           0x05,
-                              0x03,           0x03,           notevalue % 256,
-                              notevalue >> 8, duration % 256, 0x00};
-  message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
-               message[7];
-
-  uint32_t pocet;
-  pocet = write(HCom, &message, 9);
-}
-
-void CKobuki::startCommunication(char *portname, bool CommandsEnabled,
-                                 void *userDataL) {
-  connect(portname);
-  enableCommands(CommandsEnabled);
-  userData = userDataL;
-
-  int pthread_result;
-  pthread_result =
-      pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this);
-  if (pthread_result != 0) {
-    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;
-    }
-    int ok = parser.parseKobukiMessage(parser.data, message);
-
-    // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
-    // citat
-    if (ok == 0) {
-      loop(userData, parser.data);
-    }
-    free(message);
-  }
-  return 0;
-}
-
-// int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data)
-// {
-//     int rtrnvalue = checkChecksum(data);
-//     // ak je zly checksum,tak kaslat na to
-//     if (rtrnvalue != 0)
-//         return -2;
-
-//     int checkedValue = 1;
-//     // kym neprejdeme celu dlzku
-//     while (checkedValue < data[0])
-//     {
-//         // basic data subload
-//         if (data[checkedValue] == 0x01)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x0F)
-//                 return -1;
-//             checkedValue++;
-//             output.timestamp = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.BumperCenter =
-//             data[checkedValue] && 0x02; output.BumperLeft =
-//             data[checkedValue] && 0x04; output.BumperRight =
-//             data[checkedValue] && 0x01; checkedValue++; output.WheelDropLeft
-//             = data[checkedValue] && 0x02; output.WheelDropRight =
-//             data[checkedValue] && 0x01; checkedValue++; output.CliffCenter =
-//             data[checkedValue] && 0x02; output.CliffLeft = data[checkedValue]
-//             && 0x04; output.CliffRight = data[checkedValue] && 0x01;
-//             checkedValue++;
-//             output.EncoderLeft = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.EncoderRight =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 2; output.PWMleft = data[checkedValue]; checkedValue++;
-//             output.PWMright = data[checkedValue];
-//             checkedValue++;
-//             output.ButtonPress = data[checkedValue];
-//             checkedValue++;
-//             output.Charger = data[checkedValue];
-//             checkedValue++;
-//             output.Battery = data[checkedValue];
-//             checkedValue++;
-//             output.overCurrent = data[checkedValue];
-//             checkedValue++;
-//         }
-//         else if (data[checkedValue] == 0x03)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x03)
-//                 return -3;
-//             checkedValue++;
-//             output.IRSensorRight = data[checkedValue];
-//             checkedValue++;
-//             output.IRSensorCenter = data[checkedValue];
-//             checkedValue++;
-//             output.IRSensorLeft = data[checkedValue];
-//             checkedValue++;
-//         }
-//         else if (data[checkedValue] == 0x04)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x07)
-//                 return -4;
-//             checkedValue++;
-//             output.GyroAngle = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.GyroAngleRate =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 5; // 3 unsued
-//         }
-//         else if (data[checkedValue] == 0x05)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x06)
-//                 return -5;
-//             checkedValue++;
-//             output.CliffSensorRight =
-//                 data[checkedValue + 1] * 256 + data[checkedValue];
-//             checkedValue += 2;
-//             output.CliffSensorCenter =
-//                 data[checkedValue + 1] * 256 + data[checkedValue];
-//             checkedValue += 2;
-//             output.CliffSensorLeft =
-//                 data[checkedValue + 1] * 256 + data[checkedValue];
-//             checkedValue += 2;
-//         }
-//         else if (data[checkedValue] == 0x06)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x02)
-//                 return -6;
-//             checkedValue++;
-//             output.wheelCurrentLeft = data[checkedValue];
-//             checkedValue++;
-//             output.wheelCurrentRight = data[checkedValue];
-//             checkedValue++;
-//         }
-//         else if (data[checkedValue] == 0x0A)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x04)
-//                 return -7;
-//             checkedValue++;
-//             output.extraInfo.HardwareVersionPatch = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.HardwareVersionMinor = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.HardwareVersionMajor = data[checkedValue];
-//             checkedValue += 2;
-//         }
-//         else if (data[checkedValue] == 0x0B)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x04)
-//                 return -8;
-//             checkedValue++;
-//             output.extraInfo.FirmwareVersionPatch = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.FirmwareVersionMinor = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.FirmwareVersionMajor = data[checkedValue];
-//             checkedValue += 2;
-//         }
-//         else if (data[checkedValue] == 0x0D)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] % 2 != 0)
-//                 return -9;
-//             checkedValue++;
-//             output.frameId = data[checkedValue];
-//             checkedValue++;
-//             int howmanyFrames = data[checkedValue] / 3;
-//             checkedValue++;
-//             output.gyroData.reserve(howmanyFrames);
-//             output.gyroData.clear();
-//             for (int hk = 0; hk < howmanyFrames; hk++)
-//             {
-//                 TRawGyroData temp;
-//                 temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
-//                 checkedValue += 2;
-//                 temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
-//                 checkedValue += 2;
-//                 temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
-//                 checkedValue += 2;
-//                 output.gyroData.push_back(temp);
-//             }
-//         }
-//         else if (data[checkedValue] == 0x10)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x10)
-//                 return -10;
-//             checkedValue++;
-//             output.digitalInput = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.analogInputCh0 =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 2; output.analogInputCh1 = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.analogInputCh2 =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 2; output.analogInputCh3 = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 8; // 2+6
-//         }
-//         else if (data[checkedValue] == 0x13)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x0C)
-//                 return -11;
-//             checkedValue++;
-//             output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256
-//             +
-//                                      data[checkedValue + 2] * 256 * 256 +
-//                                      data[checkedValue + 1] * 256 +
-//                                      data[checkedValue];
-//             checkedValue += 4;
-//             output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256
-//             +
-//                                      data[checkedValue + 2] * 256 * 256 +
-//                                      data[checkedValue + 1] * 256 +
-//                                      data[checkedValue];
-//             checkedValue += 4;
-//             output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256
-//             +
-//                                      data[checkedValue + 2] * 256 * 256 +
-//                                      data[checkedValue + 1] * 256 +
-//                                      data[checkedValue];
-//             checkedValue += 4;
-//         }
-//         else
-//         {
-//             checkedValue++;
-//             checkedValue += data[checkedValue] + 1;
-//         }
-//     }
-//     return 0;
-// }
-
-long double CKobuki::gyroToRad(signed short GyroAngle) {
-
-  long double rad;
-  if (GyroAngle < 0) {
-    rad = GyroAngle + 360;
-  } else {
-    rad = GyroAngle;
-  }
-  return (long double)rad * PI / 180.0;
-}
-
-long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
-  if (iterationCount == 0) {
-    prevLeftEncoder = Kobuki_data.EncoderLeft;
-    prevRightEncoder = Kobuki_data.EncoderRight;
-    prevTimestamp = Kobuki_data.timestamp;
-    prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
-    iterationCount++;
-  }
-
-  int dLeft;
-  if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000) {
-    dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder +
-            (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536);
-  } else {
-    dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder;
-  }
-
-  int dRight;
-  if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000) {
-    dRight = Kobuki_data.EncoderRight - prevRightEncoder +
-             (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536);
-  } else {
-    dRight = Kobuki_data.EncoderRight - prevRightEncoder;
-  }
-
-  long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle);
-
-  if (dGyroTheta > PI) {
-    dGyroTheta -= 2 * PI;
-  }
-  if (dGyroTheta < -1 * PI) {
-    dGyroTheta += 2 * PI;
-  }
-
-  gyroTheta += dGyroTheta;
-
-  uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp;
-
-  long double mLeft = dLeft * tickToMeter;
-  long double mRight = dRight * tickToMeter;
-
-  if (mLeft == mRight) {
-    x = x + mRight;
-  } else {
-    x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
-                (sin((mRight - mLeft) / b + theta) - sin(theta));
-    y = y + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
-                (cos((mRight - mLeft) / b + theta) - cos(theta));
-    theta = (mRight - mLeft) / b + theta;
-  }
-
-  displacement = (mRight + mLeft) / 2;
-  integratedGyroTheta = integratedGyroTheta + dGyroTheta;
-  gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2);
-
-  gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2);
-
-  totalLeft += dLeft;
-  totalRight += dRight;
-
-  // ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo?
-  directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1);
-  directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1);
-  dTimestamp = (Kobuki_data.timestamp < prevTimestamp
-                    ? prevTimestamp - Kobuki_data.timestamp + 65536
-                    : dTimestamp);
-
-  prevLeftEncoder = Kobuki_data.EncoderLeft;
-  prevRightEncoder = Kobuki_data.EncoderRight;
-  prevTimestamp = Kobuki_data.timestamp;
-  prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
-
-  //    std::cout << "X: " << x
-  //              << " Y: " << y
-  //              << " Theta: " << theta
-  //              << "Gyro theta:" << gyroTheta
-  //              << std::endl;
-
-  static long counter = 0;
-
-  vectorX.push_back(gx);
-  vectorY.push_back(gy);
-  vectorGyroTheta.push_back(gyroTheta);
-
-  // if (counter % 100 == 0) {
-  //     p.plot_data(vectorY, vectorX);
-  // }
-  counter++;
-
-  return 0;
-}
-
-// 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
-void CKobuki::goStraight(long double distance) {
-  long double u_translation =
-      0; // controlled magnitude, speed of the robot in motion
-  long double w_translation = distance; // requested value
-
-  // controller parameters
-  long double Kp_translation = 4500;
-  long double e_translation = 0;
-  int upper_thresh_translation = 600;
-  int lower_thresh_translation = 40;
-  int translation_start_gain = 20;
-
-  long double u_rotation = 0; // controlled magnitude
-  long double w_rotation = 0;
-  long double Kp_rotation = 57;
-  long double e_rotation = 0;
-
-  x = 0;
-  y = 0;
-  theta = 0;
-
-  long i = 5;
-  // send command and hold until robot reaches point
-  while (fabs(x - w_translation) > 0.005 && x < w_translation) {
-    e_translation = w_translation - x;
-    u_translation = Kp_translation * e_translation;
-
-    e_rotation = w_rotation - theta;
-    if (!e_rotation == 0)
-      u_rotation = Kp_rotation / e_rotation;
-
-    // limit translation speed
-    if (u_translation > upper_thresh_translation)
-      u_translation = upper_thresh_translation;
-    if (u_translation < lower_thresh_translation)
-      u_translation = lower_thresh_translation;
-
-    // rewrite starting speed with line
-    if (i < u_translation) {
-      u_translation = i;
-    }
-
-    if (fabs(u_rotation) > 32767) {
-      u_rotation = -32767;
-    }
-
-    if (u_rotation == 0) {
-      u_rotation = -32767;
-    }
-    // send command to robot
-    this->setArcSpeed(u_translation, u_rotation);
-
-    // increment starting speed
-    i = i + translation_start_gain;
-  }
-  this->setTranslationSpeed(0);
-}
-
-/// the method performs the rotation, it rotates using the regulator, the
-/// gyroscope serves as feedback, because it is much more accurate than encoders
-void CKobuki::doRotation(long double th) {
-  long double u =
-      0; // controlled variable, angular speed of the robot during movement
-  long double w = th; // desired value in radians
-  long double Kp = PI;
-  long double e = 0;
-  long double thresh = PI / 2;
-
-  theta = 0;
-  x = 0;
-  y = 0;
-  gyroTheta = 0;
-
-  long double i = 0;
-
-  if (w > 0) {
-    while (gyroTheta < w) {
-      e = w - gyroTheta;
-      u = Kp * e;
-
-      if (u > thresh) {
-        u = thresh;
-      }
-      if (u < 0.4) {
-        u = 0.4;
-      }
-      if (i < u) {
-        u = i;
-      }
-
-      std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
-      this->setRotationSpeed(-1 * u);
-      usleep(25 * 1000);
-      i = i + 0.1;
-    }
-  } else {
-    while (gyroTheta > w) {
-      e = w - gyroTheta;
-      u = Kp * e * -1;
-
-      if (u > thresh)
-        u = thresh;
-      if (u < 0.4)
-        u = 0.4;
-
-      if (i < u) {
-        u = i;
-      }
-
-      std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
-      this->setRotationSpeed(u);
-      usleep(25 * 1000);
-      i = i + 0.1;
-    }
-  }
-
-  std::cout << "Rotation done" << std::endl;
-  this->setRotationSpeed(0);
-  usleep(25 * 1000);
-}
-
-// combines navigation to a coordinate and rotation by an angle, performs
-// movement to the selected coordinate in the robot's coordinate system
-void CKobuki::goToXy(long double xx, long double yy) {
-  long double th;
-
-  yy = yy * -1;
-
-  th = atan2(yy, xx);
-  doRotation(th);
-
-  long double s = sqrt(pow(xx, 2) + pow(yy, 2));
-
-  // resetnem suradnicovu sustavu robota
-  x = 0;
-  y = 0;
-  iterationCount = 0;
-  theta = 0;
-
-  // std::cout << "mam prejst: " << s << "[m]" << std::endl;
-
-  goStraight(s);
-
-  usleep(25 * 1000);
-  return;
-}
-
-void CKobuki::forward(int speedvalue, long double distance) {
-  // Use the goStraight logic to determine the speed and distance
-
-  // Calculate the actual speed and radius values based on the conversion table
-  int actual_speed = speedvalue;
-  int actual_radius = 0; // Pure translation (straight line)
-
-  unsigned char message[11] = {
-      0xaa,               // Start byte 1
-      0x55,               // Start byte 2
-      0x08,               // Payload length (the first 2 bytes dont count)
-      0x01,               // payload type (0x01 = control command)
-      0x04,               // Control byte or additional identifier
-      actual_speed % 256, // Lower byte of speed value
-      actual_speed >> 8,  // Upper byte of speed value
-      0x00,               // Placeholder for radius
-      0x00,               // Placeholder for radius
-      0x00                // Placeholder for checksum
-  };
-
-  // Calculate checksum
-  message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
-                message[7] ^ message[8] ^ message[9];
-
-  // Send the message
-  uint32_t pocet;
-  pocet = write(HCom, &message, 11);
-}
--- a/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
+++ b/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
@@ -1,10 +1,10 @@
 #include "KobukiParser.h"
 #include <iostream>
-
+//moet checkenvalue gebruiken of moet kijken naar de payloadlength welke dingen er extra zijn
 int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
    int rtrnvalue = checkChecksum(data);
    if (rtrnvalue != 0) {
-        // std::cerr << "Invalid checksum" << std::endl;
+        std::cerr << "Invalid checksum" << std::endl;
        return -2;
    }

@@ -108,7 +108,7 @@ void KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int
    checkedValue += 2;
    output.BumperCenter = (data[checkedValue] & 0x02) >> 1;
    output.BumperLeft = (data[checkedValue] & 0x04) >> 2;
-    output.BumperRight = data[checkedValue] & 0x01;
+    output.BumperRight = data[checkedValue] & 0x01; 
    checkedValue++;
    output.WheelDropLeft = (data[checkedValue] & 0x02) >> 1;
    output.WheelDropRight = data[checkedValue] & 0x01;
@@ -125,7 +125,9 @@ void KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int
    checkedValue++;
    output.PWMright = data[checkedValue];
    checkedValue++;
-    output.ButtonPress = data[checkedValue];
+    output.ButtonPress1  = data[checkedValue] & 0x01;
+    output.ButtonPress2 = data[checkedValue] & 0x02;
+    output.ButtonPress3 = data[checkedValue] & 0x04;
    checkedValue++;
    output.Charger = data[checkedValue];
    checkedValue++;
@@ -151,13 +153,13 @@ void KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int &
    checkedValue += 5; // 3 unused
 }

-void KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
-    output.CliffSensorRight = data[checkedValue];
-    checkedValue++;
-    output.CliffSensorCenter = data[checkedValue];
-    checkedValue++;
-    output.CliffSensorLeft = data[checkedValue];
-    checkedValue++;
+void KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue) {
+    output.CliffSensorRight = (data[checkedValue] << 8) | data[checkedValue + 1];
+    checkedValue += 2;
+    output.CliffSensorCenter = (data[checkedValue] << 8) | data[checkedValue + 1];
+    checkedValue += 2;
+    output.CliffSensorLeft = (data[checkedValue] << 8) | data[checkedValue + 1];
+    checkedValue += 2;
 }

 void KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){
--- a/src/C++/Driver/src/KobukiDriver/KobukiParser.h
+++ b/src/C++/Driver/src/KobukiDriver/KobukiParser.h
@@ -19,7 +19,8 @@ struct TKobukiData {
    int CliffCenter, CliffLeft, CliffRight;
    int EncoderLeft, EncoderRight;
    int PWMleft, PWMright;
-    int ButtonPress, Charger, Battery, overCurrent;
+    int ButtonPress1, ButtonPress2, ButtonPress3;
+    int Charger, Battery, overCurrent;
    int IRSensorRight, IRSensorCenter, IRSensorLeft;
    int GyroAngle, GyroAngleRate;
    int CliffSensorRight, CliffSensorCenter, CliffSensorLeft;
--- a/src/C++/Driver/src/MQTT/CMakeLists.txt
+++ b/src/C++/Driver/src/MQTT/CMakeLists.txt
@@ -1,30 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-set(CMAKE_CXX_STANDARD 23)
-
-# Project name
-project(mqtt_receiver)
-
-# Find the Paho MQTT C++ library
-find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
-find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
-
-# Include the headers
-include_directories(/usr/local/include)
-
-# Set source files
-set(SOURCE_FILES
-    main.cpp
-    MqttClient.cpp
-    MqttClient.h
-)
-
-# Add the executable
-add_executable(mqtt_receiver ${SOURCE_FILES})
-
-# Link the libraries
-
-# Include directories for headers
-target_include_directories(mqtt_receiver PRIVATE)
-
-find_package(Threads REQUIRED)
-target_link_libraries(mqtt_receiver Threads::Threads)
--- a/src/C++/Driver/src/MQTT/MqttClient.cpp
+++ b/src/C++/Driver/src/MQTT/MqttClient.cpp
@@ -1,61 +0,0 @@
-#include "MqttClient.h"
-
-MqttClient::MqttClient(const std::string& address, const std::string& clientId, const std::string& username, const std::string& password)
-    : client_(address, clientId), username_(username), password_(password), callback_(*this) {
-    client_.set_callback(callback_);
-    connOpts_.set_clean_session(true);
-    connOpts_.set_mqtt_version(MQTTVERSION_3_1_1);  // For MQTT 3.1.1
-    if (!username_.empty() && !password_.empty()) {
-        connOpts_.set_user_name(username_);
-        connOpts_.set_password(password_);
-    }
-}
-
-void MqttClient::connect() {
-    try {
-        std::cout << "Connecting to broker..." << std::endl;
-        client_.connect(connOpts_)->wait();
-        std::cout << "Connected!" << std::endl;
-    } catch (const mqtt::exception& exc) {
-        std::cerr << "Error: " << exc.what() << std::endl;
-        throw;
-    }
-}
-
-void MqttClient::subscribe(const std::string& topic, int qos) {
-    try {
-        std::cout << "Subscribing to topic: " << topic << std::endl;
-        client_.subscribe(topic, qos)->wait();
-    } catch (const mqtt::exception& exc) {
-        std::cerr << "Error: " << exc.what() << std::endl;
-        throw;
-    }
-}
-
-/// @brief Only needed when program doesnt keep itself alive
-void MqttClient::run() {
-    // Keep the client running to receive messages
-    while (true) {
-        std::this_thread::sleep_for(std::chrono::seconds(1));  // Wait to reduce CPU usage
-    }
-}
-
-void MqttClient::Callback::message_arrived(mqtt::const_message_ptr msg) {
-    std::lock_guard<std::mutex> lock(client_.messageMutex_);
-    client_.lastMessage_ = msg->to_string();
-}
-
-void MqttClient::Callback::connection_lost(const std::string& cause) {
-    std::cerr << "Connection lost. Cause: " << cause << std::endl;
-}
-
-void MqttClient::Callback::delivery_complete(mqtt::delivery_token_ptr token) {
-    std::cout << "Message delivered!" << std::endl;
-}
-
-/// @brief Get the last message received from the MQTT broker
-/// @return The last message received in a string
-std::string MqttClient::getLastMessage() {
-    std::lock_guard<std::mutex> lock(messageMutex_);
-    return lastMessage_;
-}
--- a/src/C++/Driver/src/MQTT/MqttClient.h
+++ b/src/C++/Driver/src/MQTT/MqttClient.h
@@ -1,38 +0,0 @@
-#ifndef MQTTCLIENT_H
-#define MQTTCLIENT_H
-
-#include <iostream>
-#include <thread>
-#include <mutex>
-#include <mqtt/async_client.h>
-
-class MqttClient {
-public:
-    MqttClient(const std::string& address, const std::string& clientId, const std::string& username = "", const std::string& password = "");
-    void connect();
-    void subscribe(const std::string& topic, int qos = 1);
-    void run();
-    std::string getLastMessage();
-
-private:
-    class Callback : public virtual mqtt::callback {
-    public:
-        Callback(MqttClient& client) : client_(client) {}
-        void message_arrived(mqtt::const_message_ptr msg) override;
-        void connection_lost(const std::string& cause) override;
-        void delivery_complete(mqtt::delivery_token_ptr token) override;
-
-    private:
-        MqttClient& client_;
-    };
-
-    mqtt::async_client client_;
-    mqtt::connect_options connOpts_;
-    Callback callback_;
-    std::string username_;
-    std::string password_;
-    std::string lastMessage_;
-    std::mutex messageMutex_;
-};
-
-#endif // MQTTCLIENT_H
--- a/src/C++/Driver/src/MQTT/example.cpp
+++ b/src/C++/Driver/src/MQTT/example.cpp
@@ -1,10 +0,0 @@
-#include "MqttClient.h"
-
-int main(){
-    MqttClient client("mqtt://localhost:1883", "raspberry_pi_client", "ishak", "kobuki");
-    client.connect();
-    client.subscribe("home/commands");
-    client.run();
-
-    return 0;
-}
--- a/src/C++/Driver/src/KobukiDriver/graph.h
+++ b/src/C++/Driver/src/KobukiDriver/graph.h
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -1,79 +1,136 @@
+#include "CKobuki.h"
 #include <iostream>
 #include <cmath>
 #include <thread>
-#include "KobukiDriver/graph.h"
-#include "MQTT/MqttClient.h"
-#include "KobukiDriver/CKobuki.h"
-
+#include "graph.h"

 using namespace std;
 CKobuki robot;
 int movement();
-std::string readMQTT();
-MqttClient client("mqtt://localhost:1883", "KobukiRPI", "ishak", "kobuki");
+int checkCenterCliff();
+void logToFile();

-
-void setup(){
-	unsigned char *null_ptr(0);
-	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
-    client.connect();
-    client.subscribe("home/commands");
-
-}
-
-int main(){
-	setup();
-	while(true){
-	readMQTT();
-	}
-	client.run();
-	return 0;
-}
-
-std::string readMQTT()
+int main()
 {
-        std::string message = client.getLastMessage();
-        if (!message.empty()) {
-            std::cout << "MQTT Message: " << message << std::endl;
-        }
+    unsigned char *null_ptr(0);
+    robot.startCommunication("/dev/ttyUSB0", true, null_ptr);

-        // Add a small delay to avoid busy-waiting
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-		return message;
+    std::thread safety([&robot]()
+                       { robot.robotSafety(); }); // use a lambda function to call the member function
+    safety.detach();
+
+    thread movementThread(movement);
+    movementThread.join(); // so the program doesnt quit
+    return 0;
 }

+int checkCenterCliff()
+{
+    while (true)
+    {
+        std::cout << robot.parser.data.CliffSensorRight << endl;
+    }
+}

 int movement()
 {
-	int text;
-	while (true)
-	{
-		cin >> text;
+    int text;
+    while (true)
+    {
+        cout << "gimme input: ";
+        cin >> text;

-		if (text == 1)
-		{
-			robot.forward(1024, 1);
-		}
-		else if (text == 2)
-		{
-			// 1 is full circle
-			robot.doRotation(0.25);
-		}
-		else if (text == 3)
-		{
-			// Add your code here for text == 3
-		}
-		else
-		{
-			try
-			{
-				robot.doRotation(text);
-				throw "NaN";
-			}
-			catch (const char *msg)
-			{
-				cerr << msg << endl;
-			}
-		}
-	}
+        if (text == 1)
+        {
+            robot.forward(400);
+        }
+        else if (text == 2)
+        {
+            // 1 is full circle
+            robot.Rotate(90);
+        }
+        else if (text == 3)
+        {
+            // Add your code here for text == 3
+        }
+        else
+        {
+            try
+            {
+                robot.doRotation(text);
+                throw "NaN";
+            }
+            catch (const char *msg)
+            {
+                cerr << msg << endl;
+            }
+        }
+    }
 }
+
+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
+            // 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
+    }
+}
--- a/src/C++/Driver/src/test.cpp
+++ b/src/C++/Driver/src/test.cpp
@@ -1,61 +1,67 @@
 #include "CKobuki.h"
-#include "graph.h"
-#include <cmath>
 #include <iostream>
+#include <cmath>
 #include <thread>
+#include "graph.h"

 using namespace std;
-
-// Globale instantie van de CKobuki-klasse
 CKobuki robot;
-
-// Functieprototypes
 int command();
-int checkCenterCliff();
-int checkWheelDrop();

-int main() {
-  // Start communicatie met de robot
-  unsigned char *null_ptr(0);
-  robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
-  usleep(1 * 1000 * 1000);
-
-  // Start een nieuwe thread voor de command-functie
-  thread mv(command);
-  usleep(30 * 1000 * 1000);
-  mv.join(); // Wacht tot de command-thread klaar is
+const int forward = 1;
+const int ROTATE = 2;

+int main()
+{
+	unsigned char *null_ptr(0);
+	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+	usleep(1 * 1000 * 1000);
+	thread mv(command);
+	usleep(30 * 1000 * 1000);
+	mv.join(); //only exit once thread one is done running
 }

-// Functie om commando's van de gebruiker te verwerken
-int command() {
-  int input;
+// int checkCenterCliff()
+// {
+// 	while (true)
+// 	{
+// 		std::cout << "cliffsensordata:" << robot.parser.data.CliffSensorCenter << std::endl;
+// 	}
+// }

-  while (true) {
-    // Vraag de gebruiker om een commando
-    std::cout << "choose between forward = 1 or rotate = 2" << endl;
-    std::cout << "What must the robot do?";
-    std::cin >> input;
+// int checkWheelDrop(){
+// 	while (true)
+// 	{
+// 		std::cout << "wheeldropdata:" << robot.parser.data.WheelDropLeft << std::endl;
+// 	}
+// }

-    // Verwerk de invoer van de gebruiker
-    switch (input) {
-    case 1: {
-      int distance;
-      std::cout << "Enter distance to move forward: ";
-      std::cin >> distance;
-      robot.goStraight(distance); // Beweeg de robot naar voren
-    } break;
+int command(){
+	cout << "choose between forward and rotate" << endl;
+	cout << "What must the robot do?";
+	cin >> input;

-    case 2: {
-      int angle;
-      std::cout << "Enter angle to rotate in degrees: ";
-      std::cin >> angle;
-      robot.doRotation(angle); // Draai de robot
-    } break;
+	switch(input){
+		case forward:{
+			int distance;
+			std::cout >> "Enter distance to move forward: ";
+			std::cin >> distance;
+			robot.goStraight(distance);
+		}
+			
+		case ROTATE:{
+			int angle;
+			std::cout >> "Enter angle to rotate: ";
+			std::cin >> angle;
+			robot.doRotation(angle);
+		}
+			robot.goStraight(-1);
+			break;
+			
+		default:
+			cout << "Invalid input" << endl;
+			break;
+	}
+	
+}

-    default:
-      cout << "Invalid input" << endl; // Ongeldige invoer
-      break;
-    }
-  }
-}
--- a/src/C++/MQTT/CMakeLists.txt
+++ b/src/C++/MQTT/CMakeLists.txt
@@ -1,15 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-set(CMAKE_CXX_STANDARD 23)
-
-# Find the Paho MQTT C++ library
-find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
-find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
-
-# Include the headers
-include_directories(/usr/local/include)
-
-# Add the executable
-add_executable(my_program main.cpp)
-
-# Link the libraries
-target_link_libraries(my_program ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY})
--- a/src/C++/MQTT/main.cpp
+++ b/src/C++/MQTT/main.cpp
@@ -1,56 +0,0 @@
-#include <iostream>
-#include <mqtt/async_client.h>
-#include <thread>  // Voor std::this_thread::sleep_for
-#include <chrono>  // Voor std::chrono::seconds
-
-const std::string ADDRESS("mqtt://localhost:1883");  // Aanpassen indien nodig
-const std::string CLIENT_ID("raspberry_pi_client");
-const std::string TOPIC("home/commands");
-
-class callback : public virtual mqtt::callback {
-  void message_arrived(mqtt::const_message_ptr msg) override {
-    std::cout << "Ontvangen bericht: '" << msg->get_topic()
-              << "' : " << msg->to_string() << std::endl;
-    // Doe iets met het bericht, bijvoorbeeld een GP.IO-activering
-  }
-
-  void connection_lost(const std::string& cause) override {
-    std::cerr << "Verbinding verloren. Oorzaak: " << cause << std::endl;
-  }
-
-  void delivery_complete(mqtt::delivery_token_ptr token) override {
-    std::cout << "Bericht afgeleverd!" << std::endl;
-  }
-};
-
-int main() {
-  mqtt::async_client client(ADDRESS, CLIENT_ID);
-  callback cb;
-  client.set_callback(cb);
-
-  mqtt::connect_options connOpts;
-  connOpts.set_clean_session(true);
-  connOpts.set_user_name("ishak");
-  connOpts.set_password("kobuki");
-  connOpts.set_mqtt_version(MQTTVERSION_3_1_1);  // Voor MQTT 3.1.1
-
-  try {
-    std::cout << "Verbinden met broker..." << std::endl;
-    client.connect(connOpts)->wait();
-    std::cout << "Verbonden!" << std::endl;
-
-    std::cout << "Abonneren op topic: " << TOPIC << std::endl;
-    client.subscribe(TOPIC, 1)->wait();
-
-    // Houd de client draaiende om berichten te blijven ontvangen
-    while (true) {
-      std::this_thread::sleep_for(std::chrono::seconds(1));  // Wacht om CPU-gebruik te verminderen
-    }
-
-  } catch (const mqtt::exception &exc) {
-    std::cerr << "Fout: " << exc.what() << std::endl;
-    return 1;
-  }
-
-  return 0;
-}
--- a/src/C++/Socket/a.out
+++ b/src/C++/Socket/a.out
--- a/src/C++/Socket/main.cpp
+++ b/src/C++/Socket/main.cpp
@@ -0,0 +1,53 @@
+#include <iostream>
+#include <boost/asio.hpp>
+#include <string>
+
+using boost::asio::ip::tcp;
+
+int main() {
+    try {
+        // Create an io_context object
+        boost::asio::io_context io_context;
+
+        // Resolve the server address and port
+        tcp::resolver resolver(io_context);
+        tcp::resolver::results_type endpoints = resolver.resolve("127.0.0.1", "4024");
+
+        // Create and connect the socket
+        tcp::socket socket(io_context);
+        boost::asio::connect(socket, endpoints);
+
+        std::cout << "Connected to the server." << std::endl;
+
+        // Receive initial message from the server
+        boost::asio::streambuf buffer;
+        boost::asio::read_until(socket, buffer, "\n");
+        std::istream is(&buffer);
+        std::string initial_message;
+        std::getline(is, initial_message);
+        std::cout << "Initial message from server: " << initial_message << std::endl;
+
+        // Send and receive messages
+        while (true) {
+            // Send a message to the server
+            std::string message;
+            std::cout << "Enter message: ";
+            std::getline(std::cin, message);
+            message += "\n"; // Add newline to mark the end of the message
+            boost::asio::write(socket, boost::asio::buffer(message));
+
+            // Receive a response from the server
+            boost::asio::streambuf response_buffer;
+            boost::asio::read_until(socket, response_buffer, "\n");
+            std::istream response_stream(&response_buffer);
+            std::string reply;
+            std::getline(response_stream, reply);
+            std::cout << "Reply from server: " << reply << std::endl;
+        }
+
+    } catch (std::exception& e) {
+        std::cerr << "Exception: " << e.what() << std::endl;
+    }
+
+    return 0;
+}
--- a/src/Python/flask/web/pycache/app.cpython-311.pyc
+++ b/src/Python/flask/web/pycache/app.cpython-311.pyc
--- a/src/Python/flask/web/app.py
+++ b/src/Python/flask/web/app.py
@@ -1,32 +1,17 @@
-from flask import Flask, request, render_template, jsonify
-import paho.mqtt.client as mqtt
+from flask import Flask, render_template

 app = Flask(__name__)

-# Create an MQTT client instance
-mqtt_client = mqtt.Client()
-mqtt_client.username_pw_set("ishak", "kobuki")
-mqtt_client.connect("localhost", 1883, 60)
-mqtt_client.loop_start()
-
-@app.route('/', methods=["POST"])
+@app.route('/')
 def index():
    return render_template('index.html')

-@app.route('/move', methods=['POST'])
-def move():
-    # Get the direction from the form data
-    direction = request.form['direction']
-    
-    # Publish the direction to the MQTT topic "home/commands"
-    result = mqtt_client.publish("home/commands", direction)
-    
-    # Check if the publish was successful
-    if result.rc == mqtt.MQTT_ERR_SUCCESS:
-        return jsonify({"message": "Bericht succesvol gepubliceerd"}), 200
-    else:
-        return jsonify({"message": "Fout bij het publiceren van bericht"}), 500
+@app.route('/control', methods=['POST'])
+def control():
+    return("hello")
+

-# Run the Flask application in debug mode
 if __name__ == '__main__':
-    app.run(debug=True)
+    app.run(debug=True)
+
+
--- a/src/Python/flask/web/static/script.js
+++ b/src/Python/flask/web/static/script.js
@@ -1,14 +0,0 @@
-window.onload = function () {
-  const form = document.getElementById("form");
-  form.onclick = function (event) {
-    event.preventDefault();
-    fetch("/move", {
-      method: "GET",
-    })
-      .then((response) => {})
-      .then(() => {
-        fetch
-      });
-  };
-};
-9
--- a/src/Python/flask/web/static/style.css
+++ b/src/Python/flask/web/static/style.css
@@ -10,7 +10,7 @@ body {
 .navbar {
  display: flex;
  justify-content: space-between;
-  max-width: 80%;
+  max-width: 70rem;
  background-color: #fff;
  border: 1px solid #f0f0f0;
  border-radius: 50px;
@@ -56,6 +56,8 @@ body {
    position: relative;
    width: 150px;
    height: 150px;
+    margin-left: auto;
+    margin-right: auto;
 }

 .btn {
--- a/src/Python/flask/web/templates/index.html
+++ b/src/Python/flask/web/templates/index.html
@@ -1,30 +1,19 @@
-{%extends 'base.html'%} {%block head%} {%endblock%} {%block content%}
-<!DOCTYPE html>
-<html lang="en">
-  <head>
-    <meta charset="UTF-8" />
-    <meta name="viewport" content="width=device-width, initial-scale=1.0" />
-    <title>Kobuki</title>
-  </head>
-  <body>
-    <div class="container">
-      <div class="image-section">
-        <img src="kobuki.jpg" alt="Kobuki Robot" id="robot-image" />
-      </div>
-      <div class="button-section">
-        <form id = "form" action="/" method="post">
-          <button class="btn" name="direction" value="left">←</button>
-          <button class="btn" name="direction" value="up">↑</button>
-          <button class="btn" name="direction" value="right">→</button>
-          <button class="btn" name="direction" value="down">↓</button>
-        </form>
-      </div>
-    </div>
-    <div class="container">
-      <h1>Sensor Data</h1>
-    </div>
-    <script href="../static/script.js"></script>
-  </body>
-</html>
+{%extends 'base.html'%}
+
+{%block head%}

 {%endblock%}
+
+{%block content%}
+<div class="container">
+    <div class="image-section">
+        <img src="kobuki.jpg" alt="Kobuki Robot" id="robot-image">
+    </div>
+    <div class="button-section">
+        <button class="btn">1</button>
+        <button class="btn">2</button>
+        <button class="btn">3</button>
+        <button class="btn">4</button>
+    </div>
+</div>
+{%endblock%}
--- a/src/Python/socket/socketServer.py
+++ b/src/Python/socket/socketServer.py
@@ -0,0 +1,20 @@
+import socket
+
+HOST = "127.0.0.1"  # Listen on all available interfaces
+PORT = 4024  # Port to listen on (non-privileged ports are > 1023)
+
+with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+    s.bind((HOST, PORT))
+    s.listen()
+    print(f"Server listening on {HOST}:{PORT}")
+    conn, addr = s.accept()
+    with conn:
+        print(f"Connected by {addr}")
+        conn.sendall(b"hallo\n")
+        while True:
+            data = conn.recv(2048)
+            if data:
+                print("Received:", repr(data))
+                conn.sendall(b"message received\n")
+            if not data:
+                break
Author	SHA1	Message	Date
Yannick van Etten	466a8659d0	Code toegevoegd voor de sgp32 sensor zodat deze wordt geïnitialiseerd	2025-01-14 17:43:27 +01:00
Yannick van Etten	523ccd87e3	Removed callback line	2024-12-18 15:07:02 +01:00
Yannick van Etten	c905bda662	Removed network information	2024-12-18 13:23:13 +01:00
Yannick van Etten	d865a50951	Kleine spellingsfouten veranderd en de callback weggehaald	2024-12-17 17:10:40 +01:00
Yannick van Etten	d7a643460c	Begin gemaakt aan data verzenden via mqtt websocket	2024-12-17 16:51:50 +01:00
Yannick van Etten	f021ebebcb	Merge branch '22-als-developer-wil-ik-een-weerstation-hebben-op-de-robot-waarmee-ik-data-van-de-omgeving-kan' of gitlab.fdmci.hva.nl:technische-informatica-sm3/ti-projectten/rooziinuubii79 into 22-als-developer-wil-ik-een-weerstation-hebben-op-de-robot-waarmee-ik-data-van-de-omgeving-kan	2024-12-16 14:35:28 +01:00
Yannick van Etten	32e0583c1b	Documentatie gemaakt de behuizing van de sensoren	2024-12-16 14:35:05 +01:00
Yannick van Etten	c0cf6f1360	De data in een json payload gezet die vervolgens verstuurt wordt naar de raspberry pi	2024-12-12 16:40:24 +01:00
Yannick van Etten	730d2b47b8	Reconnect functie toegevoegd	2024-12-12 16:09:40 +01:00
Yannick van Etten	4c5cea71b2	Restructure want de vorige was niet goed gegaan	2024-12-10 13:59:39 +01:00
Yannick van Etten	dcd3f1006d	restructure en code toegevoegd voor mqtt	2024-12-10 13:30:42 +01:00
Yannick van Etten	4f79695d9c	Foto toegevoegd van het aansluitschema	2024-11-29 14:04:12 +01:00
Yannick van Etten	b2d233386c	uitleg toegevoegd voor de MQ5 gassensor	2024-11-29 13:52:27 +01:00
Yannick van Etten	160b43f49e	nog stukje toegevoegd aan de tvoc/eco2 sensor uitleg	2024-11-19 15:26:46 +01:00
Yannick van Etten	afe8ae3357	uitleg toegevoegd van de tvoc/eco2 sensor toegevoegd	2024-11-19 15:23:40 +01:00
Yannick van Etten	66d5444488	Uitleg van de DHT11 sensor toegevoegd aan sensoronderzoek	2024-11-19 15:07:17 +01:00
Yannick van Etten	bcbd6743fb	Added comment	2024-11-18 11:16:53 +01:00
Yannick van Etten	9997bf2319	Removed double code	2024-11-18 11:12:02 +01:00
Yannick van Etten	0ae0b83e4b	Merged m5 code into sensor code	2024-11-18 11:09:13 +01:00
Yannick van Etten	db8758e1e5	removed unnecessary code	2024-11-13 14:45:27 +01:00
Yannick van Etten	08dff8bbc1	changed test code for the m5stack sensor	2024-11-13 14:37:14 +01:00
Yannick van Etten	f0260c1a91	changed file location	2024-11-13 13:46:04 +01:00
Yannick van Etten	40d601a35b	Name change and small changes	2024-11-13 13:02:22 +01:00
Yannick van Etten	3525479b17	Added comments	2024-11-11 10:53:10 +01:00
Yannick van Etten	2e08bf7e74	Removed baseline	2024-11-11 09:19:08 +01:00
Yannick van Etten	12c25b33a7	Added test code for M5 sensor. (work in progress)	2024-10-31 16:27:30 +01:00
Yannick van Etten	5b27974d5a	Added comments	2024-10-31 15:27:17 +01:00
Yannick van Etten	f4ce50db18	Name change	2024-10-31 15:12:00 +01:00
Yannick van Etten	9bd9b6c4b2	Added code for the MQ5 gas sensor	2024-10-30 18:04:58 +01:00
Yannick van Etten	c08f1e434c	rewrote DHT11 code	2024-10-30 15:35:03 +01:00
Yannick van Etten	fb35d2a84d	Code toegevoegd voor de DHT11 sensor	2024-10-29 15:17:27 +01:00
Yannick van Etten	b67c51e56b	Documentatie uitgebreid	2024-10-29 14:06:29 +01:00
Yannick van Etten	8b03438d47	Start Sensoronderzoek	2024-10-28 15:23:32 +01:00
ishak jmilou.ishak	c4a694e601	moved venv to the .gitignore	2024-10-24 14:07:11 +02:00
Sam Hos	d78389992e	added a comment	2024-10-22 15:33:19 +02:00
Sam Hos	7fa04a5c35	made it so we can read indiviual buttons	2024-10-22 15:10:08 +02:00
Sam Hos	1175444abf	edited cmakelist	2024-10-21 13:09:46 +02:00
Sam Hos	79f21f8755	testing	2024-10-21 12:34:03 +02:00
Sam Hos	9935375a5a	comment	2024-10-21 10:41:57 +02:00
Sam Hos	d41e6432b3	fixed safety of robot and that it always drives when its told to drive	2024-10-21 10:41:48 +02:00
Sam Hos	b5f5491222	attempt to make function so robot doenst throw itself from a cliff	2024-10-20 17:35:55 +02:00
Sam Hos	856d60eea2	Junk removal	2024-10-18 16:22:47 +02:00
Sam Hos	6a0024ebb8	rotation sort of works now	2024-10-16 16:43:24 +02:00
Sam Hos	0011096977	INfrastructure update	2024-10-14 14:34:45 +02:00
Sam Hos	5ead72c47e	uitleg welke sensoren gebruiken en waarom	2024-10-14 13:57:48 +02:00
ishak jmilou.ishak	216b28d27c	added template user and learning story	2024-10-14 12:10:03 +02:00
Sam Hos	b5e619ac0a	attempt to make robot rotate	2024-10-09 14:24:23 +02:00
Sam Hos	c38491ce20	added to parser so we can read raw sensordata from cliffsensor (thanks anwar)	2024-10-08 15:05:41 +02:00
Sam Hos	c9d128ecd1	todo commnets from ferry at expert review	2024-10-08 14:27:38 +02:00
Mees Roelofsz	927126a797	Merge branch 'main' of ssh://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79	2024-10-08 13:53:27 +02:00
Mees Roelofsz	6235af77b9	changed word	2024-10-08 13:53:25 +02:00
Sam Hos	9a4cd473a6	added and fixed logger	2024-10-08 13:29:24 +02:00
Mees Roelofsz	acd4b9589c	To Be Determined	2024-10-08 13:13:27 +02:00
Mees Roelofsz	c129ae4980	added more tabs	2024-10-08 13:09:45 +02:00
Mees Roelofsz	cdbb538238	decision written out	2024-10-08 13:05:34 +02:00
Mees Roelofsz	26cc0e2041	finished brainstorm	2024-10-08 12:59:34 +02:00
Sam Hos	2bee0ffc25	fix nonsense errors	2024-10-08 12:53:22 +02:00
Sam Hos	cede3689f4	cleanup and logger creation	2024-10-08 12:39:31 +02:00