diff --git a/.gitignore b/.gitignore index b82396b..d747781 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,4 @@ src/C++/Driver/vgcore* src/C++/Driver/cmake_install.cmake src/C++/Driver/Makefile src/C++/Driver/log +build/ diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index 2f132e5..8326432 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -613,3 +613,30 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ uint32_t pocet; pocet = write(HCom, &message, 11); } + +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]; + + // 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 551c28e..d4c498c 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/src/C++/Driver/src/CKobuki.h @@ -74,6 +74,7 @@ public: void forward(int speedvalue, long double distance); void doRotation(long double th); void goToXy(long double xx, long double yy); + void Rotate(int degrees); std::ofstream odometry_log; KobukiParser parser; diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index d8fcacb..c14005c 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -17,9 +17,10 @@ int main() // thread mv(movement); // mv.join(); //only exit once thread one is done running // checkCenterCliff(); - logToFile(); + // logToFile(); //seperate thread so sleep doesnt block main thread - thread logger(logToFile); + // thread logger(logToFile); + robot.Rotate(120); return 0; } @@ -27,7 +28,7 @@ int checkCenterCliff() { while (true) { - std::cout << robot.parser.data.CliffCenter << endl; + std::cout << robot.parser.data.CliffSensorRight << endl; } }