From 644a375647188bb252b1fd4cdbf554f44b6d4070 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 02:31:58 +0200 Subject: [PATCH] proximity --- TCPServer.cpp | 43 +++++++++++++++++++------------------------ TCPServer.h | 2 ++ 2 files changed, 21 insertions(+), 24 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5ad59ab..1a764cd 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -133,9 +133,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->stopEmergency = true; - if (!handleEmergencyFlag) { - std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1]) / 100); }).detach(); - } + // if (!handleEmergencyFlag) { + // std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1]) / 100); }).detach(); + // } } else if (tokens[1] != "strat") { this->broadcastMessage(message, clientSocket); @@ -802,6 +802,13 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; + if (stopEmergency) { + while (stopEmergency) { + usleep(100'000); + } + this->broadcastMessage(lastArduinoCommand); + awaitRobotIdle(); + } if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; @@ -912,7 +919,7 @@ std::vector TCPServer::getNotFallenFlowers() const { } void TCPServer::handleEmergency(int distance, double angle) { - this->handleEmergencyFlag = true; + /*this->handleEmergencyFlag = true; this->broadcastMessage("strat;arduino;clear;2\n"); @@ -926,7 +933,7 @@ void TCPServer::handleEmergency(int distance, double angle) { this->stopEmergency = false; - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(1));*/ /*double newAngle = this->robotPose.theta + angle; double newX = this->robotPose.pos.x + 200 * std::cos(newAngle); @@ -934,7 +941,7 @@ void TCPServer::handleEmergency(int distance, double angle) { usleep(200'000); this->go(newX, newY); awaitRobotIdle();*/ - } + /*} this->broadcastMessage("strat;arduino;clear;4\n"); try { @@ -949,7 +956,7 @@ void TCPServer::handleEmergency(int distance, double angle) { this->gameThread.detach(); - this->handleEmergencyFlag = false; + this->handleEmergencyFlag = false;*/ } void TCPServer::startTestAruco(const int pince) { @@ -1713,47 +1720,44 @@ void TCPServer::checkpoint(const StratPattern sp) { template void TCPServer::go(X x, Y y) { - if (stopEmergency) return; + lastArduinoCommand = "strat;arduino;go;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "\n"; this->broadcastMessage("strat;arduino;go;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "\n"); } template void TCPServer::go(std::array data) { - if (stopEmergency) return; + lastArduinoCommand = "strat;arduino;go;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "\n"; this->broadcastMessage("strat;arduino;go;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "\n"); } template void TCPServer::rotate(X angle) { - if (stopEmergency) return; + lastArduinoCommand = "strat;arduino;angle;" + std::to_string(static_cast(angle * 100)) + "\n"; this->broadcastMessage("strat;arduino;angle;" + std::to_string(static_cast(angle * 100)) + "\n"); } void TCPServer::setSpeed(const int speed) { - if (stopEmergency) return; this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); this->speed = speed; } void TCPServer::setMaxSpeed() { - if (stopEmergency) return; this->setSpeed(MAX_SPEED); } void TCPServer::setMinSpeed() { - if (stopEmergency) return; this->setSpeed(MIN_SPEED); } template void TCPServer::transit(X x, Y y, const int endSpeed) { - if (stopEmergency) return; + lastArduinoCommand = "strat;arduino;transit;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(endSpeed) + "\n"; this->broadcastMessage("strat;arduino;transit;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(endSpeed) + "\n"); } template void TCPServer::transit(std::array data, const int endSpeed) { - if (stopEmergency) return; + lastArduinoCommand = "strat;arduino;transit;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(endSpeed) + "\n"; this->broadcastMessage("strat;arduino;transit;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(endSpeed) + "\n"); } @@ -1798,42 +1802,34 @@ void TCPServer::setPosition(const Position pos, const std::string &toSend) { } void TCPServer::baisserBras() { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); } void TCPServer::transportBras() { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;transport bras;1\n"); } void TCPServer::leverBras() { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); } void TCPServer::openPince(int pince) { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n"); } void TCPServer::fullyOpenPince(int pince) { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;ouvrir total pince;" + std::to_string(pince) + "\n"); } void TCPServer::middlePince(int pince) { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;middle pince;" + std::to_string(pince) + "\n"); } void TCPServer::closePince(int pince) { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n"); } void TCPServer::checkPanneau(int servo_moteur) { - if (stopEmergency) return; this->broadcastMessage("strat;servo_moteur;check panneau;" + std::to_string(servo_moteur) + "\n"); } @@ -1842,7 +1838,6 @@ void TCPServer::uncheckPanneau(int servo_moteur) { } void TCPServer::askLidarPosition() { - if (stopEmergency) return; this->broadcastMessage("start;lidar;get pos;1\n"); } diff --git a/TCPServer.h b/TCPServer.h index 2eb1062..1d9b6a7 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -177,6 +177,8 @@ private: int lidarGetPosTimeout = 0; + std::string lastArduinoCommand{}; + public: explicit TCPServer(int port);