From b5f54912223f966ccdc20e3d0ce25d957abd81db Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Sun, 20 Oct 2024 17:35:55 +0200 Subject: [PATCH] attempt to make function so robot doenst throw itself from a cliff --- src/C++/Driver/src/CKobuki.cpp | 35 +++++++++++++++++++++++++++++++++- src/C++/Driver/src/CKobuki.h | 2 ++ 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index 0c30c74..8cf4219 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -586,6 +586,9 @@ void CKobuki::goToXy(long double xx, long double yy) return; } +/// @brief Makes the Kobuki go forward +/// @param speedvalue speed of robot in mm/s +/// @param distance distance in meters void CKobuki::forward(int speedvalue, long double distance) { // Use the goStraight logic to determine the speed and distance @@ -615,6 +618,8 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ pocet = write(HCom, &message, 11); } +/// @brief Makes the kobuki rotate +/// @param degrees Rotation in degrees void CKobuki::Rotate(int degrees) { // convert raidans to degrees @@ -625,7 +630,7 @@ void CKobuki::Rotate(int degrees) { // 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); @@ -636,3 +641,31 @@ void CKobuki::Rotate(int degrees) { 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) + { + sendNullMessage(); + forward(100, -0.5); + }; + }; + +} + +void CKobuki::sendNullMessage(){ + + unsigned char message[5] = { + 0xaa, // Start byte 1 + 0x55, // Start byte 2 + 0x00, // Payload length (the first 2 bytes dont count) + 0x00 // Placeholder for checksum +}; + +// Calculate checksum +message[4] = message[2] ^ message[3]; + + // Send the message + uint32_t pocet; + pocet = write(HCom, &message, 5); +} diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h index 37f8ded..408a376 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/src/C++/Driver/src/CKobuki.h @@ -76,6 +76,8 @@ public: void goToXy(long double xx, long double yy); void Rotate(int degrees); std::ofstream odometry_log; + void robotSafety(); + void sendNullMessage(); KobukiParser parser;