diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
index 587c50c..e2e1b39 100755
--- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
@@ -592,6 +592,8 @@ void CKobuki::robotSafety() {
       std::cout << "Safety condition triggered!" << std::endl; // Debug print
       forward(-100); // reverse the robot
     }
+    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(100)));
+
   }
 }
 
diff --git a/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp b/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
index df3d32d..80b5acc 100644
--- a/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
+++ b/src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
@@ -2,6 +2,8 @@
 #include <iostream>
 //moet checkenvalue gebruiken of moet kijken naar de payloadlength welke dingen er extra zijn
 int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(20))); //avoid busy waiting. The kobuki sends a message every 20ms
+
     int rtrnvalue = checkChecksum(data);
     if (rtrnvalue != 0) {
         // std::cerr << "Invalid checksum" << std::endl;
diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 197acb4..2a00a55 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -37,6 +37,9 @@ int main()
     
     while(true){
         parseMQTT(readMQTT());
+        std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(10)));
+
+        
     }
 
     sendMqtt.join();