diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index 8cf4219..ae909b8 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -589,7 +589,7 @@ void CKobuki::goToXy(long double xx, long double yy) /// @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) { +void CKobuki::forward(int speedvalue) { // Use the goStraight logic to determine the speed and distance // Calculate the actual speed and radius values based on the conversion table @@ -616,6 +616,8 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ // Send the message uint32_t pocet; pocet = write(HCom, &message, 11); + pocet = write(HCom, &message, 11); + } /// @brief Makes the kobuki rotate @@ -642,30 +644,39 @@ void CKobuki::Rotate(int degrees) { } -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::robotSafety() { + while (true) { + if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight || + parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight) { + std::cout << "Safety condition triggered!" << std::endl; // Debug print + forward(-100); // reverse the robot + + } + + } } void CKobuki::sendNullMessage(){ - unsigned char message[5] = { + unsigned char message[11] = { 0xaa, // Start byte 1 0x55, // Start byte 2 - 0x00, // Payload length (the first 2 bytes dont count) + 0x08, // Payload length (the first 2 bytes dont count) + 0x01, // payload type (0x01 = control command) + 0x04, // Control byte or additional identifier + 0x00, // Lower byte of speed value + 0x00, // Upper byte of speed value + 0x00, // Placeholder for radius + 0x00, // Placeholder for radius 0x00 // Placeholder for checksum }; -// Calculate checksum -message[4] = message[2] ^ message[3]; + +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, 5); + pocet = write(HCom, &message, 11); } diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h index 408a376..3efddf2 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/src/C++/Driver/src/CKobuki.h @@ -71,7 +71,7 @@ public: // control functions void goStraight(long double distance); - void forward(int speedvalue, long double distance); + void forward(int speedvalue); void doRotation(long double th); void goToXy(long double xx, long double yy); void Rotate(int degrees); diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 7b33dd1..b5fe243 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -15,13 +15,11 @@ 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(90); + std::thread safety([&robot]() { robot.robotSafety(); }); // use a lambda function to call the member function + safety.detach(); + thread movementThread(movement); + + movementThread.join(); return 0; } @@ -38,11 +36,13 @@ int movement() int text; while (true) { + cout << "gimme input: "; cin >> text; if (text == 1) { - robot.forward(1024, 1); + robot.forward(400); + } else if (text == 2) {