diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index aac0049..cb138c4 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -789,3 +789,32 @@ void CKobuki::goToXy(long double xx, long double yy) usleep(25 * 1000); return; } + +void CKobuki::forward(int speedvalue, long double distance) { + // Use the goStraight logic to determine the speed and distance + + // Calculate the actual speed and radius values based on the conversion table + int actual_speed = speedvalue; + int actual_radius = 0; // Pure translation (straight line) + +unsigned char message[11] = { + 0xaa, // Start byte 1 + 0x55, // Start byte 2 + 0x0a, // Payload length + 0x01, // amount of payloads + 0x04, // Control byte or additional identifier + actual_speed % 256, // Lower byte of speed value + actual_speed >> 8, // Upper byte of speed value + 0x00, // Placeholder for radius + 0x00, // 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]; + + // Send the message + uint32_t pocet; + pocet = write(HCom, &message, 11); +} diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h index 5c41c25..bf0a75c 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/src/C++/Driver/src/CKobuki.h @@ -154,6 +154,7 @@ public: // control functions void goStraight(long double distance); + void forward(int speedvalue, long double distance); void doRotation(long double th); void goToXy(long double xx, long double yy); std::ofstream odometry_log; diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index d922d69..e8030bd 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -36,7 +36,7 @@ int movement() if (text == 1) { - robot.goStraight(1); + robot.forward(1024, 100); } else if (text == 2) {