diff --git a/src/C++/Driver/CMakeLists.txt b/src/C++/Driver/CMakeLists.txt
index 7bb90d4..ced9869 100644
--- a/src/C++/Driver/CMakeLists.txt
+++ b/src/C++/Driver/CMakeLists.txt
@@ -2,11 +2,13 @@ cmake_minimum_required(VERSION 3.9)
 project(kobuki_control)
 
 set(SOURCE_FILES
-        src/KobukiParser.cpp
-        src/KobukiParser.h
-        src/CKobuki.cpp
-        src/CKobuki.h
-        src/test.cpp)
+        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)
 
 add_executable(kobuki_control ${SOURCE_FILES})
 #target_link_libraries(kobuki_control )
\ No newline at end of file
diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
similarity index 100%
rename from src/C++/Driver/src/CKobuki.cpp
rename to src/C++/Driver/src/KobukiDriver/CKobuki.cpp
diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/KobukiDriver/CKobuki.h
similarity index 100%
rename from src/C++/Driver/src/CKobuki.h
rename to src/C++/Driver/src/KobukiDriver/CKobuki.h
diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
similarity index 100%
rename from src/C++/Driver/src/KobukiParser.cpp
rename to src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
diff --git a/src/C++/Driver/src/KobukiParser.h b/src/C++/Driver/src/KobukiDriver/KobukiParser.h
similarity index 100%
rename from src/C++/Driver/src/KobukiParser.h
rename to src/C++/Driver/src/KobukiDriver/KobukiParser.h
diff --git a/src/C++/Driver/src/graph.h b/src/C++/Driver/src/KobukiDriver/graph.h
similarity index 100%
rename from src/C++/Driver/src/graph.h
rename to src/C++/Driver/src/KobukiDriver/graph.h
diff --git a/src/C++/MQTT/mqttclass/CMakeLists.txt b/src/C++/Driver/src/MQTT/CMakeLists.txt
similarity index 100%
rename from src/C++/MQTT/mqttclass/CMakeLists.txt
rename to src/C++/Driver/src/MQTT/CMakeLists.txt
diff --git a/src/C++/MQTT/mqttclass/MqttClient.cpp b/src/C++/Driver/src/MQTT/MqttClient.cpp
similarity index 77%
rename from src/C++/MQTT/mqttclass/MqttClient.cpp
rename to src/C++/Driver/src/MQTT/MqttClient.cpp
index c2b2fc9..7923822 100644
--- a/src/C++/MQTT/mqttclass/MqttClient.cpp
+++ b/src/C++/Driver/src/MQTT/MqttClient.cpp
@@ -1,11 +1,11 @@
 #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) {
+    : 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()) { //if username and password are not empty also set those otherwise log in anonymously
+    if (!username_.empty() && !password_.empty()) {
         connOpts_.set_user_name(username_);
         connOpts_.set_password(password_);
     }
@@ -32,7 +32,7 @@ void MqttClient::subscribe(const std::string& topic, int qos) {
     }
 }
 
-/// @brief only needed if program doesnt loop itself
+/// @brief Only needed when program doesnt keep itself alive
 void MqttClient::run() {
     // Keep the client running to receive messages
     while (true) {
@@ -41,9 +41,8 @@ void MqttClient::run() {
 }
 
 void MqttClient::Callback::message_arrived(mqtt::const_message_ptr msg) {
-    std::cout << "Received message: '" << msg->get_topic()
-              << "' : " << msg->to_string() << std::endl;
-    // Do something with the message, e.g., trigger a GPIO action
+    std::lock_guard<std::mutex> lock(client_.messageMutex_);
+    client_.lastMessage_ = msg->to_string();
 }
 
 void MqttClient::Callback::connection_lost(const std::string& cause) {
@@ -52,4 +51,11 @@ void MqttClient::Callback::connection_lost(const std::string& cause) {
 
 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_;
 }
\ No newline at end of file
diff --git a/src/C++/MQTT/mqttclass/MqttClient.h b/src/C++/Driver/src/MQTT/MqttClient.h
similarity index 79%
rename from src/C++/MQTT/mqttclass/MqttClient.h
rename to src/C++/Driver/src/MQTT/MqttClient.h
index 3f11986..0cae258 100644
--- a/src/C++/MQTT/mqttclass/MqttClient.h
+++ b/src/C++/Driver/src/MQTT/MqttClient.h
@@ -3,6 +3,7 @@
 
 #include <iostream>
 #include <thread>
+#include <mutex>
 #include <mqtt/async_client.h>
 
 class MqttClient {
@@ -11,13 +12,18 @@ public:
     void connect();
     void subscribe(const std::string& topic, int qos = 1);
     void run();
+    std::string getLastMessage();
 
 private:
     class Callback : public virtual mqtt::callback {
     public:
-        void message_arrived(mqtt::const_message_ptr msg) override; //overide the main functions in the original class
+        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_;
@@ -25,6 +31,8 @@ private:
     Callback callback_;
     std::string username_;
     std::string password_;
+    std::string lastMessage_;
+    std::mutex messageMutex_;
 };
 
 #endif // MQTTCLIENT_H
\ No newline at end of file
diff --git a/src/C++/MQTT/mqttclass/main.cpp b/src/C++/Driver/src/MQTT/example.cpp
similarity index 100%
rename from src/C++/MQTT/mqttclass/main.cpp
rename to src/C++/Driver/src/MQTT/example.cpp
diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 62d4cec..fd7b1ae 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -1,31 +1,47 @@
-#include "CKobuki.h"
 #include <iostream>
 #include <cmath>
 #include <thread>
-#include "graph.h"
+#include "KobukiDriver/graph.h"
+#include "MQTT/MqttClient.h"
+#include "KobukiDriver/CKobuki.h"
+
 
 using namespace std;
 CKobuki robot;
 int movement();
-int checkCenterCliff();
+std::string ReadMQTT();
+MqttClient client("mqtt://localhost:1883", "KobukiRPI", "ishak", "kobuki");
 
-int main()
-{
+
+void setup(){
 	unsigned char *null_ptr(0);
 	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
-	// thread mv(movement);
-	// mv.join(); //only exit once thread one is done running
+    client.connect();
+    client.subscribe("home/commands");
 
-	checkCenterCliff();
+}
+
+int main()
+	setup();
+{
+
+readMQTT();
 
 	return 0;
 }
 
-int checkCenterCliff()
+std::string ReadMQTT()
 {
-	while(true){
-	std::cout << robot.parser.data.CliffSensorCenter << endl;
-	}
+    while (true) {
+        std::string message = client.getLastMessage();
+        if (!message.empty()) {
+            std::cout << "MQTT Message: " << message << std::endl;
+        }
+
+        // Add a small delay to avoid busy-waiting
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+		return message;
+    }
 }
 
 
diff --git a/src/Python/flask/web/app.py b/src/Python/flask/web/app.py
index 085eba0..3647d55 100644
--- a/src/Python/flask/web/app.py
+++ b/src/Python/flask/web/app.py
@@ -10,7 +10,7 @@ mqtt_client.username_pw_set("ishak", "kobuki")
 mqtt_client.connect("localhost", 1883, 60)
 mqtt_client.loop_start()
 
-@app.route('/')
+@app.route('/', methods=["POST"])
 def index():
     return render_template('index.html')
 
@@ -24,4 +24,4 @@ def move():
         return jsonify({"message": "Fout bij het publiceren van bericht"}), 500
 
 if __name__ == '__main__':
-    app.run(debug=True)
\ No newline at end of file
+    app.run(debug=True)