diff --git a/TCPServer.cpp b/TCPServer.cpp index 8b6dea0..076e781 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -596,7 +596,6 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;speed;130\n"); this->broadcastMessage("strat;arduino;go;762,0\n"); usleep(3'000'000); - this->broadcastMessage("strat;arduino;speed;200\n"); for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { @@ -609,6 +608,12 @@ void TCPServer::startGameBlueTeam() { usleep(100'000); } } + + this->broadcastMessage("strat;arduino;speed;200\n"); + + toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y + 350)) + "\n"; + this->broadcastMessage(toSend); + awaitRobotIdle(); } /* @@ -626,45 +631,8 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;angle;-157\n"); awaitRobotIdle(); - timeout = 0; - found = false; - while (!found) { - for (const auto & arucoTag : this->arucoTags) { - 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; - break; - } - } - } - - if (!found) { - this->broadcastMessage("start;aruco;get aruco;1\n"); - 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; - } - - toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y - 350)) + "\n"; - this->broadcastMessage(toSend); - awaitRobotIdle(); - - this->broadcastMessage("strat;arduino;angle;157\n"); - awaitRobotIdle(); + arucoTags.clear(); + this->broadcastMessage("strat;aruco;get aruco;1\n"); timeout = 0; found = false; @@ -706,6 +674,52 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;angle;157\n"); awaitRobotIdle(); + arucoTags.clear(); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + + timeout = 0; + found = false; + while (!found) { + for (const auto & arucoTag : this->arucoTags) { + 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; + break; + } + } + } + + if (!found) { + this->broadcastMessage("start;aruco;get aruco;1\n"); + 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; + } + + toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y - 350)) + "\n"; + this->broadcastMessage(toSend); + awaitRobotIdle(); + + this->broadcastMessage("strat;arduino;angle;157\n"); + awaitRobotIdle(); + + arucoTags.clear(); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + timeout = 0; found = false; while (!found) { @@ -746,7 +760,7 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;angle;314\n"); awaitRobotIdle(); - std::vector pinceHavePurpleFlower; + pinceHavePurpleFlower.clear(); for (int i = 0; i < 3; i++) { if (pinceState[i] == PURPLE_FLOWER) { pinceHavePurpleFlower.push_back(i); @@ -800,6 +814,12 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage(toSend); usleep(100'000); } + + this->broadcastMessage("strat;arduino;speed;200\n"); + + toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y + 350)) + "\n"; + this->broadcastMessage(toSend); + awaitRobotIdle(); } }*/ @@ -932,7 +952,7 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage("strat;arduino;angle;314\n"); awaitRobotIdle(); - std::vector pinceHavePurpleFlower; + pinceHavePurpleFlower.clear(); for (int i = 0; i < 3; i++) { if (pinceState[i] == PURPLE_FLOWER) { pinceHavePurpleFlower.push_back(i); @@ -986,6 +1006,12 @@ void TCPServer::startGameBlueTeam() { this->broadcastMessage(toSend); usleep(100'000); } + + this->broadcastMessage("strat;arduino;speed;200\n"); + + toSend = "strat;arduino;go;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y + 350)) + "\n"; + this->broadcastMessage(toSend); + awaitRobotIdle(); } }*/