From 2f12b0ba028e89e020aaabcabe4921c787dfa65b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 17 Apr 2024 16:20:11 +0200 Subject: [PATCH] add test --- TCPServer.cpp | 250 ++++++++++++++++++++++++++++++++++++++++++++++++-- TCPServer.h | 5 +- 2 files changed, 245 insertions(+), 10 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 95be6d3..c1640d3 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -188,7 +188,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) finishPoint[0] = 400; finishPoint[1] = 400; finishPoint[2] = 3.1415; - break; case 6: this->team = YELLOW; @@ -201,6 +200,13 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) break; default: + this->team = TEST; + spawnPoint[0] = 1200; + spawnPoint[1] = 1800; + spawnPoint[2] = 0; + finishPoint[0] = 1200; + finishPoint[1] = 1800; + finishPoint[2] = 0; return; } @@ -229,6 +235,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) case YELLOW: std::thread([this]() { this->startGameYellowTeam(); }).detach(); break; + case TEST: + std::thread([this]() { this->startGameTest(); }).detach(); + break; } } else if (tokens[0] == "aruco" && tokens[2] == "get aruco") { @@ -363,7 +372,7 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGameBlueTeam() { // TODO redo that part because that give point to the other team - this->broadcastMessage("strat;arduino;speed;200\n"); + this->broadcastMessage("strat;arduino;speed;130\n"); this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); usleep(100'000); std::string toSend; @@ -377,6 +386,9 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage(toSend); awaitRobotIdle(); + this->broadcastMessage("strat;arduino;angle;0"); + awaitRobotIdle(); + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); usleep(100'000); @@ -389,6 +401,9 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage(toSend); awaitRobotIdle(); + this->broadcastMessage("strat;arduino;angle;0"); + awaitRobotIdle(); + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); usleep(100'000); @@ -396,10 +411,12 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage(toSend); awaitRobotIdle(); + this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->broadcastMessage("strat;arduino;angle;104\n"); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->broadcastMessage("strat;arduino;speed;200\n"); int timeout = 0; ArucoTag tag; @@ -607,7 +624,7 @@ void TCPServer::startGameBlueTeam() { found = false; while (!found) { for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endsWith(arucoTag.name(), "flower")) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { tag = arucoTag; found = true; @@ -647,7 +664,7 @@ void TCPServer::startGameBlueTeam() { found = false; while (!found) { for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endsWith(arucoTag.name(), "flower")) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { tag = arucoTag; found = true; @@ -687,7 +704,7 @@ void TCPServer::startGameBlueTeam() { found = false; while (!found) { for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endsWith(arucoTag.name(), "flower")) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { tag = arucoTag; found = true; @@ -793,7 +810,7 @@ void TCPServer::startGameBlueTeam() { found = false; while (!found) { for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endsWith(arucoTag.name(), "flower")) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { tag = arucoTag; found = true; @@ -833,7 +850,7 @@ void TCPServer::startGameBlueTeam() { found = false; while (!found) { for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endsWith(arucoTag.name(), "flower")) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { tag = arucoTag; found = true; @@ -873,7 +890,7 @@ void TCPServer::startGameBlueTeam() { found = false; while (!found) { for (const auto & arucoTag : this->arucoTags) { - if (TCPUtils::endsWith(arucoTag.name(), "flower")) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) { tag = arucoTag; found = true; @@ -995,6 +1012,221 @@ void TCPServer::startGameYellowTeam() { this->broadcastMessage("strat;servo_moteur;clear;1"); } +void TCPServer::startGameTest() { + this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); + this->broadcastMessage("strat;servo_moteur;fermer pince;1\n"); + this->broadcastMessage("strat;servo_moteur;fermer pince;2\n"); + this->broadcastMessage("strat;servo_moteur;ouvrir pince;0\n"); + // TODO set to 200 when the robot is ready + this->broadcastMessage("strat;arduino;speed;200\n"); + + this->broadcastMessage("strat;aruco;get aruco;1\n"); + + int timeout = 0; + ArucoTag tag; + bool found = false; + while (!found) { + for (const auto & arucoTag : this->arucoTags) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { + tag = arucoTag; + found = true; + break; + } + } + + if (!found) { + this->broadcastMessage("start;aruco;get aruco;1"); + usleep(500'000); + timeout++; + if (timeout > 10) { + return; + } + } + } + + // goToAruco(tag, 1); + + if (pinceState[1] == NONE) { + goToAruco(tag, 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag, 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag, 0); + } else { + return; + } + + // pi/4 + this->broadcastMessage("strat;arduino;angle;314\n"); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + + // ReSharper disable once CppDFAUnreachableCode + + this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); + + usleep(2'000'000); + arucoTags.clear(); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + + found = false; + timeout = 0; + while (!found) { + for (const auto & arucoTag : this->arucoTags) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { + tag = arucoTag; + found = true; + break; + } + } + + if (!found) { + this->broadcastMessage("start;aruco;get aruco;1"); + usleep(500'000); + timeout++; + if (timeout > 10) { + return; + } + } + } + + if (pinceState[1] == NONE) { + goToAruco(tag, 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag, 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag, 0); + } else { + return; + } + + this->broadcastMessage("strat;arduino;angle;157\n"); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + + // ReSharper disable once CppDFAUnreachableCode + + this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); + + usleep(2'000'000); + arucoTags.clear(); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + + found = false; + timeout = 0; + while (!found) { + for (const auto & arucoTag : this->arucoTags) { + if (TCPUtils::endWith(arucoTag.name(), "flower")) { + tag = arucoTag; + found = true; + break; + } + } + + if (!found) { + this->broadcastMessage("start;aruco;get aruco;1"); + usleep(500'000); + timeout++; + if (timeout > 10) { + return; + } + } + } + + if (pinceState[1] == NONE) { + goToAruco(tag, 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag, 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag, 0); + } else { + return; + } + + // go to jardinière + + this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); + + std::string toSend = "strat;arduino;go;762,300\n"; + this->broadcastMessage(toSend); + usleep(200'000); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + + this->broadcastMessage("strat;arduino;angle;157\n"); + usleep(200'000); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + + this->broadcastMessage("strat;arduino;speed;150\n"); + this->broadcastMessage("strat;arduino;go;762,0\n"); + usleep(1'000'000); + + this->broadcastMessage("strat;servo_moteur;ouvrir pince;0\n"); + pinceState[0] = NONE; + this->broadcastMessage("strat;servo_moteur;ouvrir pince;2\n"); + pinceState[2] = NONE; + usleep(200'000); + + this->broadcastMessage("strat;servo_moteur;fermer pince;0\n"); + this->broadcastMessage("strat;servo_moteur;fermer pince;2\n"); + this->broadcastMessage("strat;servo_moteur;ouvrir pince;1\n"); + pinceState[1] = NONE; + usleep(200'000); + + this->broadcastMessage("strat;arduino;speed;200\n"); + + toSend = "strat;arduino;go;" + std::to_string(static_cast(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast(this->endRobotPose.pos.y)) + "\n"; + this->broadcastMessage(toSend); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + + toSend = "strat;arduino;angle;" + std::to_string(static_cast(this->endRobotPose.theta * 100)) + "\n"; + this->broadcastMessage(toSend); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + + // toSend = "start;arduino;angle;" + std::to_string(this->endRobotPose.theta * 100) + "\n"; + // this->broadcastMessage(toSend); + + this->broadcastMessage("strat;servo_moteur;baisser bras;1"); + + toSend = "strat;arduino;go;" + std::to_string(static_cast(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast(this->endRobotPose.pos.y)) + "\n"; + this->broadcastMessage(toSend); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + toSend = "strat;arduino;angle;" + std::to_string(static_cast(this->endRobotPose.theta * 100)); + this->broadcastMessage(toSend); + isRobotIdle = 0; + while (this->isRobotIdle < 3) { + usleep(200'000); + this->broadcastMessage("strat;arduino;get state;1\n"); + } + + this->broadcastMessage("strat;servo_moteur;clear;1"); +} + void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double robotPosX = this->robotPose.pos.x; diff --git a/TCPServer.h b/TCPServer.h index 727216f..9e13b7e 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -21,7 +21,8 @@ struct ClientTCP enum Team { YELLOW, - BLUE + BLUE, + TEST }; class TCPServer; // Forward declaration @@ -99,6 +100,8 @@ public: void startGameYellowTeam(); + void startGameTest(); + void goToAruco(const ArucoTag &arucoTag, int pince); void askArduinoPos();