attempt to make function so robot doenst throw itself from a cliff

src/C++/Driver/src/CKobuki.cpp

@@ -586,6 +586,9 @@ void CKobuki::goToXy(long double xx, long double yy)
     return;
 }
 
+/// @brief Makes the Kobuki go forward
+/// @param speedvalue speed of robot in mm/s
+/// @param distance   distance in meters
 void CKobuki::forward(int speedvalue, long double distance) {
   // Use the goStraight logic to determine the speed and distance
 
@@ -615,6 +618,8 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
   pocet = write(HCom, &message, 11);
 }
 
+/// @brief Makes the kobuki rotate
+/// @param degrees Rotation in degrees
 void CKobuki::Rotate(int degrees) {
 
     // convert raidans to degrees
@@ -636,3 +641,31 @@ void CKobuki::Rotate(int degrees) {
     setRotationSpeed(0);
 }
 
+
+void CKobuki::robotSafety(){
+    while(true){
+        if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight || parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight)
+        {   
+            sendNullMessage();
+            forward(100, -0.5);
+        };
+    };
+
+}
+
+void CKobuki::sendNullMessage(){
+
+    unsigned char message[5] = {
+    0xaa,                // Start byte 1
+    0x55,                // Start byte 2
+    0x00,                // Payload length (the first 2 bytes dont count)
+    0x00                 // Placeholder for checksum
+};
+
+// Calculate checksum
+message[4] = message[2] ^ message[3];
+
+  // Send the message
+  uint32_t pocet;
+  pocet = write(HCom, &message, 5);
+}

src/C++/Driver/src/CKobuki.h

@@ -76,6 +76,8 @@ public:
 	void goToXy(long double xx, long double yy);
 	void Rotate(int degrees);
 	std::ofstream odometry_log;
+	void robotSafety();
+	void sendNullMessage();
 	KobukiParser parser;