From 206bf965e5246800d11e39b07e7979effdc86092 Mon Sep 17 00:00:00 2001
From: Sam Hos <sam.hos@hva.nl>
Date: Mon, 4 Nov 2024 14:42:53 +0100
Subject: [PATCH] fix safety

---
 src/C++/Driver/src/KobukiDriver/CKobuki.cpp | 2 +-
 src/C++/Driver/src/main.cpp                 | 3 +++
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
index e98ab0e..08e819e 100755
--- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
@@ -787,7 +787,7 @@ void CKobuki::robotSafety(std::string *pointerToMessage) {
         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";
+            *pointerToMessage = "estop";
             forward(-100); // reverse the robot
 
         }
diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index 86b8bb2..a06969b 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -63,6 +63,9 @@ void parseMQTT(std::string message){
         robot.sendNullMessage();
         robot.sendNullMessage();
     }
+    else if (message == "estop"){
+        robot.forward(-500);
+    }
     else{
         std::cout << "Invalid command" << std::endl;
     }