mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 11:55:00 +00:00
laat kobuki nu connecten op open ports
This commit is contained in:
@@ -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));
|
||||
}
|
||||
|
Reference in New Issue
Block a user