From 810dfd3108ba0b62cbdb1e6d9613c1f71ec6ff84 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Mon, 27 May 2024 19:09:09 +0200 Subject: [PATCH 1/7] update --- TCPServer.cpp | 59 ++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 51 insertions(+), 8 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d98ebac..9b9d7fb 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -173,12 +173,28 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (args[0] == "0") { if (!handleEmergecnyFlag) { + if (value < -23000) value = -23000; + if (value > 23000) value = 23000; + int angle; if (value < 0) { - angle = static_cast(Modelec::mapValue(value, -32767.0, -2000.0, -PI / 2, 0.0) * 100); + if (value < -23000) { + angle = -PI * 100 / 2; + } + else { + angle = static_cast(Modelec::mapValue(value, -23000.0, -2000.0, -PI / 2, 0.0) * 100); + } + } + else if (value == 0) { + angle = 0; } else { - angle = static_cast(Modelec::mapValue(value, 2000.0, 32768.0, 0.0, PI / 2) * 100); + if (value > 0) { + angle = PI * 100 / 2; + } + else { + angle = static_cast(Modelec::mapValue(value, 2000.0, 23000.0, 0.0, PI / 2) * 100); + } } this->broadcastMessage("strat;arduino;angle;" + std::to_string(angle) + "\n"); @@ -187,24 +203,38 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (args[0] == "1") { int speed; + if (value > -4000 && value < 4000) { + value = 0; + } + value = -value; if (value < 0) { - speed = static_cast(Modelec::mapValue(value, -32767.0, 0.0, -310.0, -70.0)); + if (value < -25000) { + speed = -310; + } + else { + speed = static_cast(Modelec::mapValue(value, -25000.0, -4000.0, -310.0, -70.0)); + } } else if (value == 0) { speed = 0; } else { - speed = static_cast(Modelec::mapValue(value, 0.0, 32768.0, 70.0, 310.0)); + if (value > 25000) { + speed = 310; + } + else { + speed = static_cast(Modelec::mapValue(value, 4000.0, 25000.0, 70.0, 310.0)); + } } if (!handleEmergecnyFlag) { this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); } else { - if (speed >= 0 && ((this->lidarDectectionAngle > PI / 2 && this->lidarDectectionAngle < 3 * PI / 2) || (this->lidarDecetionDistance > speed * 1.5))) { + if (speed >= 0 && (this->lidarDectectionAngle > PI / 2 && this->lidarDectectionAngle < 3 * PI / 2)) { this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); } - else if (speed <= 0 && ((this->lidarDectectionAngle < PI / 2 || this->lidarDectectionAngle > 3 * PI / 2) || (this->lidarDecetionDistance > speed * 1.5))) { + else if (speed <= 0 && (this->lidarDectectionAngle < PI / 2 || this->lidarDectectionAngle > 3 * PI / 2)) { this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); } } @@ -213,10 +243,23 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) int speed; if (value < 0) { - speed = static_cast(Modelec::mapValue(value, -32767.0, -2000.0, -310.0, 0.0)); + if (value < -25000) { + speed = -310; + } + else { + speed = static_cast(Modelec::mapValue(value, -25000.0, -2000.0, -310.0, 0.0)); + } + } + else if (value == 0) { + speed = 0; } else { - speed = static_cast(Modelec::mapValue(value, 2000.0, 32768.0, 0.0, 310.0)); + if (value > 25000) { + speed = 310; + } + else { + speed = static_cast(Modelec::mapValue(value, 2000.0, 25000.0, 0.0, 310.0)); + } } this->broadcastMessage("start;arduino;rotate;" + std::to_string(speed)); From b2e897136989627ccddb3fded207a345ac23ac49 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Tue, 28 May 2024 17:49:39 +0200 Subject: [PATCH 2/7] update --- TCPServer.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9b9d7fb..55a35b1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -173,8 +173,10 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (args[0] == "0") { if (!handleEmergecnyFlag) { - if (value < -23000) value = -23000; - if (value > 23000) value = 23000; + if (value < -15000) value = -15000; + if (value > 15000) value = 15000; + + if (value > -5000 && value < 5000) value = 0; int angle; if (value < 0) { @@ -182,7 +184,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) angle = -PI * 100 / 2; } else { - angle = static_cast(Modelec::mapValue(value, -23000.0, -2000.0, -PI / 2, 0.0) * 100); + angle = static_cast(Modelec::mapValue(value, -15000.0, -5000.0, -PI / 2, 0.0) * 100); } } else if (value == 0) { @@ -193,7 +195,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) angle = PI * 100 / 2; } else { - angle = static_cast(Modelec::mapValue(value, 2000.0, 23000.0, 0.0, PI / 2) * 100); + angle = static_cast(Modelec::mapValue(value, 5000.0, 15000.0, 0.0, PI / 2) * 100); } } From 2bd4c0a5474ed2ba1178a574e3c25f8c579fd3f5 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Tue, 28 May 2024 17:59:44 +0200 Subject: [PATCH 3/7] update --- TCPServer.cpp | 8 ++++++++ TCPServer.h | 2 ++ 2 files changed, 10 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 55a35b1..bae8ad3 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -280,6 +280,14 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (tokens[3] == "1") { this->togglePince(2); } + else if (tokens[3] == "11") { + this->alertLidar = true; + this->broadcastMessage("strat;gc;start proximity;1\n"); + } + else if (tokens[3] == "12") { + this->alertLidar = false; + this->broadcastMessage("strat;gc;stop proximity;1\n"); + } else if (tokens[3] == "13") { this->togglePanel(0); } diff --git a/TCPServer.h b/TCPServer.h index 00ed3c8..001d888 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -93,6 +93,8 @@ private: int lidarDecetionDistance = 0; + bool alertLidar = true; + public: explicit TCPServer(int port); From 7a42a148487c715aea82415800b029ea4a172c0f Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Tue, 28 May 2024 18:22:54 +0200 Subject: [PATCH 4/7] up --- TCPServer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index bae8ad3..bc91344 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -154,7 +154,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (client.name == "lidar") { this->broadcastMessage("strat;lidar;start;1\n"); this->broadcastMessage("strat;lidar;set table;0\n"); - this->broadcastMessage("strat;lidar;set range;750\n"); + this->broadcastMessage("strat;lidar;set range;300\n"); lidarSocket = clientSocket; } std::cout << client.socket << " | " << client.name << " is ready" << std::endl; @@ -171,7 +171,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) value = 0; } if (args[0] == "0") { - if (!handleEmergecnyFlag) { + if (!handleEmergecnyFlag || !alertLidar) { if (value < -15000) value = -15000; if (value > 15000) value = 15000; @@ -229,7 +229,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } } - if (!handleEmergecnyFlag) { + if (!handleEmergecnyFlag || !alertLidar) { this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); } else { @@ -282,11 +282,11 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } else if (tokens[3] == "11") { this->alertLidar = true; - this->broadcastMessage("strat;gc;start proximity;1\n"); + this->broadcastMessage("strat;gc;start proximity alert;1\n"); } else if (tokens[3] == "12") { this->alertLidar = false; - this->broadcastMessage("strat;gc;stop proximity;1\n"); + this->broadcastMessage("strat;gc;stop proximity alert;1\n"); } else if (tokens[3] == "13") { this->togglePanel(0); From 6ffacc39e4e502551057fb81408560ccef776e3f Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Tue, 28 May 2024 20:01:02 +0200 Subject: [PATCH 5/7] update --- TCPServer.cpp | 14 ++++++-------- TCPServer.h | 2 +- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index bc91344..96a5a3c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -130,7 +130,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->lidarDectectionAngle = stod(args[1]) / 100; - if (!handleEmergecnyFlag) { + if (!handleEmergencyFlag) { std::thread([this]() { handleEmergency(); }).detach(); } } @@ -155,6 +155,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->broadcastMessage("strat;lidar;start;1\n"); this->broadcastMessage("strat;lidar;set table;0\n"); this->broadcastMessage("strat;lidar;set range;300\n"); + this->broadcastMessage("strat;gc;nonvalid borne;2000\n"); lidarSocket = clientSocket; } std::cout << client.socket << " | " << client.name << " is ready" << std::endl; @@ -167,11 +168,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (tokens[2] == "axis") { std::vector args = Modelec::split(tokens[3], ","); double value = std::stod(args[1]); - if (value > -2000 && value < 2000) { - value = 0; - } if (args[0] == "0") { - if (!handleEmergecnyFlag || !alertLidar) { + if (!handleEmergencyFlag || !alertLidar) { if (value < -15000) value = -15000; if (value > 15000) value = 15000; @@ -229,7 +227,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } } - if (!handleEmergecnyFlag || !alertLidar) { + if (!handleEmergencyFlag || !alertLidar) { this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); } else { @@ -423,7 +421,7 @@ size_t TCPServer::nbClients() const { void TCPServer::handleEmergency() { - handleEmergecnyFlag = true; + handleEmergencyFlag = true; this->sendToClient("strat;arduino;clear;1\n", arduinoSocket); this->sendToClient("strat;arduino;clear;1\n", arduinoSocket); @@ -432,7 +430,7 @@ void TCPServer::handleEmergency() { stopEmergency = false; usleep(300'000); } - handleEmergecnyFlag = false; + handleEmergencyFlag = false; } diff --git a/TCPServer.h b/TCPServer.h index 001d888..326bbfb 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -78,7 +78,7 @@ private: bool stopEmergency = false; - bool handleEmergecnyFlag = false; + bool handleEmergencyFlag = false; int arduinoSocket = -1; int lidarSocket = -1; From 7588cdec01ce7cd88ed12829a3c88f475c3fc768 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Sat, 1 Jun 2024 08:58:33 +0200 Subject: [PATCH 6/7] clear if value = 0 --- TCPServer.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 96a5a3c..b1160b6 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -125,7 +125,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) lidarDecetionDistance = stoi(args[0]); // TODO distance de detection proportionnelle a la vitesse - if (lidarDecetionDistance < 300) { + if (lidarDecetionDistance < 400) { stopEmergency = true; this->lidarDectectionAngle = stod(args[1]) / 100; @@ -154,7 +154,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (client.name == "lidar") { this->broadcastMessage("strat;lidar;start;1\n"); this->broadcastMessage("strat;lidar;set table;0\n"); - this->broadcastMessage("strat;lidar;set range;300\n"); + this->broadcastMessage("strat;lidar;set range;400\n"); this->broadcastMessage("strat;gc;nonvalid borne;2000\n"); lidarSocket = clientSocket; } @@ -186,7 +186,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } } else if (value == 0) { - angle = 0; + this->broadcastMessage("strat;arduino;clear;1\n"); + return; } else { if (value > 0) { @@ -217,7 +218,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) speed = static_cast(Modelec::mapValue(value, -25000.0, -4000.0, -310.0, -70.0)); } } else if (value == 0) { - speed = 0; + this->broadcastMessage("strat;arduino;clear;1\n"); + return; } else { if (value > 25000) { speed = 310; @@ -284,6 +286,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } else if (tokens[3] == "12") { this->alertLidar = false; + this->handleEmergencyFlag = false; this->broadcastMessage("strat;gc;stop proximity alert;1\n"); } else if (tokens[3] == "13") { From f5941b20464b76985c4bebd9f99d0264c7663261 Mon Sep 17 00:00:00 2001 From: bap-0-1 Date: Sat, 1 Jun 2024 10:41:50 +0200 Subject: [PATCH 7/7] update --- TCPServer.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b1160b6..01a53f9 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -186,8 +186,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } } else if (value == 0) { - this->broadcastMessage("strat;arduino;clear;1\n"); - return; + angle = 0; } else { if (value > 0) {