diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index cbfa3dc..e5c7f22 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -12,6 +12,8 @@ void parseMQTT(std::string message);
 MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); // create a client object
 std::string message = "stop";
 std::string serializeKobukiData(const TKobukiData &data);
+void sendKobukiData(TKobukiData &data);
+
 void setup()
 {
     unsigned char *null_ptr(0);
@@ -23,13 +25,13 @@ void setup()
 int main()
 {
     setup();
-    // std::thread safety([&]() { robot.robotSafety(&message); });
-    // while(true){
-    // parseMQTT(readMQTT());
-    // }
-    // safety.join();
-    // return 0;
-    client.publishMessage("kobuki/data", serializeKobukiData(robot.parser.data));
+    std::thread safety([&]() { robot.robotSafety(&message); });
+    std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
+    while(true){
+    parseMQTT(readMQTT());
+    }
+    safety.join();
+    return 0;
 }
 
 std::string readMQTT()
@@ -200,4 +202,12 @@ std::string serializeKobukiData(const TKobukiData &data) {
 
     json += "]}";
     return json;
+}
+//create extra function to send the message every 100ms
+//needed it so it can be threaded
+void sendKobukiData(TKobukiData &data) {
+    while (true) {
+        client.publishMessage("kobuki/data", serializeKobukiData(data));
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    }
 }
\ No newline at end of file