diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
index a909673..e98ab0e 100755
--- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
@@ -781,6 +781,22 @@ void CKobuki::Rotate(int degrees) {
 }
 
 
+void CKobuki::robotSafety(std::string *pointerToMessage) {
+    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
+            *pointerToMessage = "stop";
+            forward(-100); // reverse the robot
+
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(100)));
+
+
+    }
+}
+
 void CKobuki::robotSafety() {
     while (true) {
 
diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.h b/src/C++/Driver/src/KobukiDriver/CKobuki.h
index 3efddf2..d303826 100755
--- a/src/C++/Driver/src/KobukiDriver/CKobuki.h
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.h
@@ -76,7 +76,8 @@ public:
 	void goToXy(long double xx, long double yy);
 	void Rotate(int degrees);
 	std::ofstream odometry_log;
-	void robotSafety();
+	void robotSafety(std::string *pointerToMessage);
+	void robotSafety(); //overload
 	void sendNullMessage();
 	KobukiParser parser;
 
diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index dd3da5f..8f85c45 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -12,26 +12,30 @@ int movement();
 std::string readMQTT();
 void parseMQTT(std::string message);
 MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); //create a client object
-
+std::string message = "stop";
 
 void setup(){
 	unsigned char *null_ptr(0);
 	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
     client.connect();
     client.subscribe("home/commands");
+
+
 }
 
 int main(){
 	setup();
+    std::thread safety([&]() { robot.robotSafety(&message); });
 	while(true){
     parseMQTT(readMQTT());
 	}
+    safety.join();
 	return 0;
 }
 
 std::string readMQTT()
 {
-        std::string message = client.getLastMessage();
+        message = client.getLastMessage();
         if (!message.empty()) {
             std::cout << "MQTT Message: " << message << std::endl;
         }
@@ -43,20 +47,21 @@ std::string readMQTT()
 
 void parseMQTT(std::string message){
     if(message == "up"){
-        robot.forward(600);
+        robot.forward(1024);
     }
     else if(message == "left"){
-        robot.Rotate(90);
+        robot.Rotate(45);
 
     }
     else if(message == "right"){
-        robot.Rotate(-90);
+        robot.Rotate(-45);
     }
     else if(message == "down"){
-        robot.forward(600);
+        robot.forward(-800);
     }
     else if(message == "stop"){
         robot.sendNullMessage();
+        this_thread::sleep_for(chrono::milliseconds(1000));
     }
     else{
         std::cout << "Invalid command" << std::endl;