avoid busy waiting

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)));
+
   }
 }
 

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;

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();