rotation sort of works now

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

@@ -3,6 +3,7 @@
 #include "termios.h"
 #include <cstddef>
 #include <iostream>
+#include <thread>
 
 
 // plot p;
@@ -614,29 +615,30 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
   pocet = write(HCom, &message, 11);
 }
 
+
+#include <cmath>
+#include <chrono>
+#include <thread>
+#include <iostream>
+#include <unistd.h>
+
 void CKobuki::Rotate(int degrees) {
-    //w is rotation speed of the robot, in [rad/s].
-	long double b = 0.23; // wheelbase distance in meters, from kobuki manual https://yujinrobot.github.io/kobuki/doxygen/enAppendixProtocolSpecification.html
-    int calculatedRotation = ((degrees * PI / 180) * b) / 2;
-    int actual_speed = 0; // Pure rotation
-unsigned char message[11] = {
-    0xaa,                // Start byte 1
-    0x55,                // Start byte 2
-    0x08,                // Payload length (the first 2 bytes dont count)
-    0x01,                // payload type (0x01 = control command)
-    0x04,                // Control byte or additional identifier
-    actual_speed % 256,  // Lower byte of speed value
-    actual_speed >> 8,   // Upper byte of speed value
-    calculatedRotation % 256,                // Placeholder for radius
-    calculatedRotation >> 8,                // Placeholder for radius
-    0x00                 // Placeholder for checksum
-};
 
-// Calculate checksum
-message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
-              message[7] ^ message[8] ^ message[9];
+    // convert raidans to degrees
+    float radians = degrees * PI / 180.0;
 
-  // Send the message
-  uint32_t pocet;
-  pocet = write(HCom, &message, 11);
+    // Calculate the rotation speed in radians per second
+    double radpersec = 1; 
+
+    // calculator rotation time and give absolute value
+    float rotation_time = std::abs(radians / radpersec);
+
+    // Use original function to set the rotation speed in mm/s
+    setRotationSpeed(radians);
+
+    // Sleep for the calculated rotation time
+    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(rotation_time * 1000)));
+
+    // Stop the robot after the rotation
+    setRotationSpeed(0);
 }

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

@@ -12,7 +12,7 @@
 ////*************************************************************************************
 #ifndef KOBUKI_CLASS_123456789
 #define KOBUKI_CLASS_123456789
-#define PI          3.141592653589793238462643383279502884L /* pi */
+#define PI  3.141592653589793238462643383279502884L /* pi */
 #define MS_INSTRUCTION_DELAY 25
 
 #include <stdio.h>

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

@@ -14,13 +14,14 @@ int main()
 {
 	unsigned char *null_ptr(0);
 	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+    sleep(1);
 	// thread mv(movement);
 	// mv.join(); //only exit once thread one is done running
 	// checkCenterCliff();
 	// logToFile();
 	//seperate thread so sleep doesnt block main thread
 	// thread logger(logToFile);
-    robot.Rotate(120);
+    robot.Rotate(90);
 	return 0;
 }