From b90bca0060dad81f905ddf771203a931f4d9b1ad Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:05:11 +0100 Subject: [PATCH 1/9] re-added robotsafety --- src/C++/Driver/src/KobukiDriver/CKobuki.cpp | 14 ++++++++++++++ src/C++/Driver/src/KobukiDriver/CKobuki.h | 3 ++- src/C++/Driver/src/main.cpp | 7 +++++-- 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp index a909673..532eb7a 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp @@ -781,6 +781,20 @@ void CKobuki::Rotate(int degrees) { } +void CKobuki::robotSafety(std::string *pointerToMessage) { + 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 + *pointerToMessage = "stop"; + forward(-100); // reverse the robot + + } + + } +} + void CKobuki::robotSafety() { while (true) { diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.h b/src/C++/Driver/src/KobukiDriver/CKobuki.h index 3efddf2..d303826 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.h +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.h @@ -76,7 +76,8 @@ public: void goToXy(long double xx, long double yy); void Rotate(int degrees); std::ofstream odometry_log; - void robotSafety(); + void robotSafety(std::string *pointerToMessage); + void robotSafety(); //overload void sendNullMessage(); KobukiParser parser; diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index dd3da5f..34b12ab 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -12,26 +12,29 @@ int movement(); std::string readMQTT(); void parseMQTT(std::string message); MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); //create a client object - +std::string message; void setup(){ unsigned char *null_ptr(0); robot.startCommunication("/dev/ttyUSB0", true, null_ptr); client.connect(); client.subscribe("home/commands"); + } int main(){ setup(); + std::thread safety([&]() { robot.robotSafety(&message); }); while(true){ parseMQTT(readMQTT()); } + safety.join(); return 0; } std::string readMQTT() { - std::string message = client.getLastMessage(); + message = client.getLastMessage(); if (!message.empty()) { std::cout << "MQTT Message: " << message << std::endl; } From 3af23f61cfd4290c287f828e36070802f31b8741 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:10:16 +0100 Subject: [PATCH 2/9] start safety thread --- src/C++/Driver/src/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 34b12ab..d61bab1 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -19,16 +19,16 @@ void setup(){ robot.startCommunication("/dev/ttyUSB0", true, null_ptr); client.connect(); client.subscribe("home/commands"); + std::thread safety([&]() { robot.robotSafety(&message); }); + } int main(){ setup(); - std::thread safety([&]() { robot.robotSafety(&message); }); while(true){ parseMQTT(readMQTT()); } - safety.join(); return 0; } From 60a51daf7bf118d4670696fff17580181dcf3e6a Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:16:10 +0100 Subject: [PATCH 3/9] added sleep --- src/C++/Driver/src/KobukiDriver/CKobuki.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp index 532eb7a..e98ab0e 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp @@ -791,6 +791,8 @@ void CKobuki::robotSafety(std::string *pointerToMessage) { forward(-100); // reverse the robot } + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(100))); + } } From 0dc267536ea58ec1f1ae87ca16242c0706a9ea67 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:19:31 +0100 Subject: [PATCH 4/9] fix nullpointer --- src/C++/Driver/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index d61bab1..3c14f48 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -12,7 +12,7 @@ int movement(); std::string readMQTT(); void parseMQTT(std::string message); MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); //create a client object -std::string message; +std::string message = "stop"; void setup(){ unsigned char *null_ptr(0); From c3270a1c473181d765f3e283f4dd40d4f644dc0d Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:23:02 +0100 Subject: [PATCH 5/9] fix thread going out of scope --- src/C++/Driver/src/main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 3c14f48..148095b 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -19,16 +19,17 @@ void setup(){ robot.startCommunication("/dev/ttyUSB0", true, null_ptr); client.connect(); client.subscribe("home/commands"); - std::thread safety([&]() { robot.robotSafety(&message); }); } int main(){ setup(); + std::thread safety([&]() { robot.robotSafety(&message); }); while(true){ parseMQTT(readMQTT()); } + safety.join(); return 0; } From 95bd144c2e00a8cb92b5a2996167698a07c65aa4 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:29:18 +0100 Subject: [PATCH 6/9] drive testing --- src/C++/Driver/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 148095b..365a612 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -47,7 +47,7 @@ std::string readMQTT() void parseMQTT(std::string message){ if(message == "up"){ - robot.forward(600); + robot.forward(1024); } else if(message == "left"){ robot.Rotate(90); From 0a43e2ef571c324e505b1f59fccf9638f55b67d2 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:31:40 +0100 Subject: [PATCH 7/9] added more preciese controls --- src/C++/Driver/src/main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 365a612..d8dfd1f 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -47,17 +47,17 @@ std::string readMQTT() void parseMQTT(std::string message){ if(message == "up"){ - robot.forward(1024); + robot.forward(400); } else if(message == "left"){ - robot.Rotate(90); + robot.Rotate(10); } else if(message == "right"){ - robot.Rotate(-90); + robot.Rotate(-10); } else if(message == "down"){ - robot.forward(600); + robot.forward(400); } else if(message == "stop"){ robot.sendNullMessage(); From f111030e7355b83af1b01d262629d6fd7aa6b07a Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 13:37:09 +0100 Subject: [PATCH 8/9] added better safety --- src/C++/Driver/src/main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index d8dfd1f..c8b5ec2 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -61,6 +61,7 @@ void parseMQTT(std::string message){ } else if(message == "stop"){ robot.sendNullMessage(); + this_thread::sleep_for(chrono::milliseconds(1000)); } else{ std::cout << "Invalid command" << std::endl; From f3033b1632bf14b61c177cb03a58207618e1bcff Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 4 Nov 2024 14:00:35 +0100 Subject: [PATCH 9/9] change rotation controls --- src/C++/Driver/src/main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index c8b5ec2..8f85c45 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -47,17 +47,17 @@ std::string readMQTT() void parseMQTT(std::string message){ if(message == "up"){ - robot.forward(400); + robot.forward(1024); } else if(message == "left"){ - robot.Rotate(10); + robot.Rotate(45); } else if(message == "right"){ - robot.Rotate(-10); + robot.Rotate(-45); } else if(message == "down"){ - robot.forward(400); + robot.forward(-800); } else if(message == "stop"){ robot.sendNullMessage();