From 6a0024ebb85fccd88e03b8274c980e387e58529b Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Wed, 16 Oct 2024 16:43:24 +0200 Subject: [PATCH] rotation sort of works now --- src/C++/Driver/src/CKobuki.cpp | 48 ++++++++++++++++++---------------- src/C++/Driver/src/CKobuki.h | 2 +- src/C++/Driver/src/main.cpp | 3 ++- 3 files changed, 28 insertions(+), 25 deletions(-) diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index 8326432..df5bfc6 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -3,6 +3,7 @@ #include "termios.h" #include #include +#include // plot p; @@ -614,29 +615,30 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ pocet = write(HCom, &message, 11); } + +#include +#include +#include +#include +#include + 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(rotation_time * 1000))); + + // Stop the robot after the rotation + setRotationSpeed(0); +} \ No newline at end of file diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h index d4c498c..37f8ded 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/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 diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index c14005c..7b33dd1 100644 --- a/src/C++/Driver/src/main.cpp +++ b/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; }