From ef573112ae8d4cc20c8daa22c593eed1a3847798 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Wed, 2 Oct 2024 17:57:04 +0200 Subject: [PATCH] sad attempt at making my own forward command --- src/C++/Driver/src/CKobuki.cpp | 29 +++++++++++++++++++++++++++++ src/C++/Driver/src/CKobuki.h | 1 + src/C++/Driver/src/main.cpp | 2 +- 3 files changed, 31 insertions(+), 1 deletion(-) 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) {