diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp index 105df8d..61022a3 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp @@ -755,3 +755,65 @@ void CKobuki::forward(int speedvalue) { uint32_t pocet; pocet = write(HCom, &message, 11); } + +/// @brief Makes the kobuki rotate +/// @param degrees Rotation in degrees +void CKobuki::Rotate(int degrees) { + + // convert raidans to degrees + float radians = degrees * PI / 180.0; + + // 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(rotation_time * 1000))); + + // Stop the robot after the rotation + 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) { + std::cout << "Safety condition triggered!" << std::endl; // Debug print + forward(-100); // reverse the robot + + } + + } +} + +void CKobuki::sendNullMessage(){ + + 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 + 0x00, // Lower byte of speed value + 0x00, // Upper byte of speed value + 0x00, // Placeholder for radius + 0x00, // Placeholder for radius + 0x00 // Placeholder for checksum +}; + + +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, 11); +} +