From ec44cb955b6a17ca258bbcd3c6f7b5ee612d56e5 Mon Sep 17 00:00:00 2001 From: "ishak jmilou.ishak" Date: Tue, 21 Jan 2025 12:29:03 +0100 Subject: [PATCH] feat: implement automatic reconnection for Kobuki when disconnected --- docs/code/kobuki-reconnect.md | 78 +++++++++++++++++++++ src/C++/Driver/src/KobukiDriver/CKobuki.cpp | 2 +- src/C++/Driver/src/KobukiDriver/CKobuki.h | 2 +- src/C++/Driver/src/main.cpp | 4 +- 4 files changed, 82 insertions(+), 4 deletions(-) create mode 100644 docs/code/kobuki-reconnect.md diff --git a/docs/code/kobuki-reconnect.md b/docs/code/kobuki-reconnect.md new file mode 100644 index 0000000..ce6a027 --- /dev/null +++ b/docs/code/kobuki-reconnect.md @@ -0,0 +1,78 @@ +# Kobuki automatische reconnect + +Mijn taak was om de kobuki automatisch te reconnecten als de verbinding verbroken werd met de pi. nu moet je telkens handmatig de pi laten connecten met de kobuki, dit is niet handig als iemand niet weet hoe dit moet. + +De connectie word gemaakt met ttyUSB0. Dat is de eerste poort die wij kunnen gebruiken voor de kobuki. + +```cpp + robot.startCommunication("/dev/ttyUSB0", true, null_ptr); +``` + +ik heb een functie gemaakt die kijkt of de pi nog is verbonden aan de kobuki. in de if statement kijkt hij elke 5 seconden of de ttyusb0 poort beschikbaar is. + +```cpp +void checkKobukiConnection() +{ + while (true) + { + std::lock_guard lock(connectionMutex); + + // Controleer of het apparaat beschikbaar is + if (!std::ifstream("/dev/ttyUSB0")){ + if (kobuki_connected){ + cout << "Kobuki disconnected: USB device not found." << endl; + kobuki_connected = false; + } + std::this_thread::sleep_for(std::chrono::seconds(5)); + continue; // Probeer later opnieuw + } +``` + +Hier kijk ik dan of de kobuki is geconnect, zoniet dan moet ie weer connecten met ttyUSB0 + +```cpp + // Controleer of de Kobuki verbonden is + if (!robot.isConnected()){ + if (kobuki_connected){ + cout << "Kobuki disconnected." << endl; + kobuki_connected = false; + } + + cout << "Attempting to reconnect Kobuki..." << endl; + robot.startCommunication("/dev/ttyUSB0", true, nullptr); + + if (robot.isConnected()){ + cout << "Kobuki reconnected successfully!" << endl; + kobuki_connected = true; + } + else{ + cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl; + } + } +``` + +Nu heb ik het probleem dat als ik de kabel eruit steek en terug in de kobuki stop dat de pi niet meer wil connecten. Dit komt omdat het systeem denkt dat de poort "ttyUSB0" nog steeds gebruikt word. Als ik de kabel dan terug stop word de poort "ttyUSB1" gebruikt, omdat ttyusb0 niet word vrijgegeven. + +```bash +ishak@raspberrypi:~ $ dmesg | tail -n 20 +[10516.084132] usb 1-1.3: Product: iClebo Kobuki +[10516.084144] usb 1-1.3: Manufacturer: Yujin Robot +[10516.084155] usb 1-1.3: SerialNumber: kobuki_AI02MQMK +[10516.091210] ftdi_sio 1-1.3:1.0: FTDI USB Serial Device converter detected +[10516.091414] usb 1-1.3: Detected FT232R +[10516.099169] usb 1-1.3: FTDI USB Serial Device converter now attached to ttyUSB1 +[10574.100491] usb 1-1.3: USB disconnect, device number 34 +[10574.101596] ftdi_sio ttyUSB1: FTDI USB Serial Device converter now disconnected from ttyUSB1 +[10574.101735] ftdi_sio 1-1.3:1.0: device disconnected +[10579.697816] usb 1-1.3: new full-speed USB device number 35 using dwc_otg +[10579.829776] usb 1-1.3: New USB device found, idVendor=0403, idProduct=6001, bcdDevice= 6.00 +[10579.829821] usb 1-1.3: New USB device strings: Mfr=1, Product=2, SerialNumber=3 +[10579.829836] usb 1-1.3: Product: iClebo Kobuki +[10579.829848] usb 1-1.3: Manufacturer: Yujin Robot +[10579.829860] usb 1-1.3: SerialNumber: kobuki_AI02MQMK +[10579.840148] ftdi_sio 1-1.3:1.0: FTDI USB Serial Device converter detected +[10579.840351] usb 1-1.3: Detected FT232R +[10579.842208] usb 1-1.3: FTDI USB Serial Device converter now attached to ttyUSB1 +[10612.745819] hwmon hwmon1: Voltage normalised +[10614.761829] hwmon hwmon1: Undervoltage detected! +``` \ No newline at end of file diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp index 7608f1b..2a38640 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp @@ -238,7 +238,7 @@ void CKobuki::setSound(int noteinHz, int duration) { pocet = write(HCom, &message, 9); } -void CKobuki::startCommunication(const char *portname, bool CommandsEnabled, +void CKobuki::startCommunication(char *portname, bool CommandsEnabled, void *userDataL) { if(connect(portname) != -1){ enableCommands(CommandsEnabled); diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.h b/src/C++/Driver/src/KobukiDriver/CKobuki.h index 18c93c4..f4df897 100755 --- a/src/C++/Driver/src/KobukiDriver/CKobuki.h +++ b/src/C++/Driver/src/KobukiDriver/CKobuki.h @@ -59,7 +59,7 @@ public: long loop(void *user_data, TKobukiData &Kobuki_data); - void startCommunication(const char *portname,bool CommandsEnabled,void *userDataL); + void startCommunication(char *portname,bool CommandsEnabled,void *userDataL); int measure(); //thread function, contains an infinite loop and reads data void setLed(int led1 = 0, int led2 = 0); //led1 green/red 2/1, //led2 green/red 2/1 void setTranslationSpeed(int mmpersec); diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 5e12164..19b8b3c 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -39,7 +39,7 @@ void setup() { std::string port = findKobukiPort(); unsigned char *null_ptr(0); - robot.startCommunication((port.c_str()), true, null_ptr); + robot.startCommunication(const_cast(port.c_str()), true, null_ptr); // connect mqtt server and sub to commands client.connect(); client.subscribe("home/commands"); @@ -95,7 +95,7 @@ void checkKobukiConnection() } cout << "Attempting to reconnect Kobuki..." << endl; - robot.startCommunication((port.c_str()), true, nullptr); + robot.startCommunication(const_cast(port.c_str()), true, nullptr); if (robot.isConnected()){ cout << "Kobuki reconnected successfully!" << endl;