fixed safety of robot and that it always drives when its told to drive

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

@@ -589,7 +589,7 @@ void CKobuki::goToXy(long double xx, long double yy)
 /// @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) {
+void CKobuki::forward(int speedvalue) {
   // Use the goStraight logic to determine the speed and distance
 
   // Calculate the actual speed and radius values based on the conversion table
@@ -616,6 +616,8 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
   // Send the message
   uint32_t pocet;
   pocet = write(HCom, &message, 11);
+  pocet = write(HCom, &message, 11);
+
 }
 
 /// @brief Makes the kobuki rotate
@@ -642,30 +644,39 @@ void CKobuki::Rotate(int degrees) {
 }
 
 
-void CKobuki::robotSafety(){
+void CKobuki::robotSafety() {
-    while(true){
+    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);
-        };
-    };
 
+        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
+            forward(-100); // reverse the robot
+
+        }
+
+    }
 }
 
 void CKobuki::sendNullMessage(){
 
-    unsigned char message[5] = {
+    unsigned char message[11] = {
     0xaa,                // Start byte 1
     0x55,                // Start byte 2
-    0x00,                // Payload length (the first 2 bytes dont count)
+    0x08,                // Payload length (the first 2 bytes dont count)
+    0x01,                // payload type (0x01 = control command)
+    0x04,                // Control byte or additional identifier
+    0x00,  // Lower byte of speed value
+    0x00,   // Upper byte of speed value
+    0x00,                // Placeholder for radius
+    0x00,                // Placeholder for radius
     0x00                 // Placeholder for checksum
 };
 
-// Calculate checksum
+
-message[4] = message[2] ^ message[3];
+message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
+              message[7] ^ message[8] ^ message[9];
 
   // Send the message
   uint32_t pocet;
-  pocet = write(HCom, &message, 5);
+  pocet = write(HCom, &message, 11);
 }

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

@@ -71,7 +71,7 @@ public:
 
 	// control functions 
 	void goStraight(long double distance);
-	void forward(int speedvalue, long double distance);
+	void forward(int speedvalue);
 	void doRotation(long double th);
 	void goToXy(long double xx, long double yy);
 	void Rotate(int degrees);

src/C++/Driver/src/main.cpp

@@ -15,13 +15,11 @@ int main()
 	unsigned char *null_ptr(0);
 	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
     sleep(1);
-	// thread mv(movement);
+    std::thread safety([&robot]() { robot.robotSafety(); }); // use a lambda function to call the member function
-	// mv.join(); //only exit once thread one is done running
+    safety.detach();
-	// checkCenterCliff();
+    thread movementThread(movement);
-	// logToFile();
+
-	//seperate thread so sleep doesnt block main thread
+    movementThread.join();
-	// thread logger(logToFile);
-    robot.Rotate(90);
 	return 0;
 }
 
@@ -38,11 +36,13 @@ int movement()
 	int text;
 	while (true)
 	{
+        cout << "gimme input: ";
 		cin >> text;
 
 		if (text == 1)
 		{
-			robot.forward(1024, 1);
+            robot.forward(400);
+
 		}
 		else if (text == 2)
 		{