laat kobuki nu connecten op open ports

This commit is contained in:
ishak jmilou.ishak
2025-01-20 15:18:37 +01:00
parent 2c630bf89b
commit 0e9d9dda68

View File

@@ -1,6 +1,7 @@
#include <iostream>
#include <thread>
#include <fstream>
#include <filesystem>
#include "MQTT/MqttClient.h"
#include "KobukiDriver/CKobuki.h"
#include <opencv4/opencv2/opencv.hpp>
@@ -21,6 +22,19 @@ std::string message = "stop";
std::string serializeKobukiData(const TKobukiData &data);
void sendKobukiData(TKobukiData &data);
std::string findKobukiPort()
{
for (const auto& entry : std::filesystem::directory_iterator("/dev"))
{
std::string device = entry.path().string();
if (device.find("ttyUSB") != std::string::npos)
{
return device; // Returneer de eerste gevonden poort
}
}
return ""; // Geen poort gevonden
}
void setup()
{
unsigned char *null_ptr(0);
@@ -61,6 +75,7 @@ void checkKobukiConnection()
while (true)
{
std::lock_guard<std::mutex> lock(connectionMutex);
std::string port = findKobukiPort();
// Controleer of het apparaat beschikbaar is
if (!std::ifstream("/dev/ttyUSB0")){
@@ -80,7 +95,7 @@ void checkKobukiConnection()
}
cout << "Attempting to reconnect Kobuki..." << endl;
robot.startCommunication("/dev/ttyUSB0", true, nullptr);
robot.startCommunication(port.c_str(), true, nullptr);
if (robot.isConnected()){
cout << "Kobuki reconnected successfully!" << endl;
@@ -89,12 +104,7 @@ void checkKobukiConnection()
else{
cout << "Failed to reconnect Kobuki, retrying in 5 seconds..." << endl;
}
}else if (!kobuki_connected){
// Update status als de verbinding hersteld is
cout << "Kobuki is connected." << endl;
kobuki_connected = true;
}
// Wacht voordat je opnieuw controleert
std::this_thread::sleep_for(std::chrono::seconds(5));
}