From 0c65575c68b10e6f51bf92afd4dc13f5d207a6d0 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 25 Apr 2024 00:17:48 +0200 Subject: [PATCH 001/316] rework strat --- TCPServer.cpp | 810 +++++++++++++++++++++++++------------------------- TCPServer.h | 77 ++++- main.cpp | 3 - utils.cpp | 16 - utils.h | 23 +- 5 files changed, 486 insertions(+), 443 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index ef591ea..a157ed3 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -42,7 +42,7 @@ void ClientHandler::closeConnection() { server->clientDisconnected(clientSocket); // Inform the server that the client has disconnected } -TCPServer::TCPServer(int port) +TCPServer::TCPServer(int port) : team(TEST) { this->robotPose = {500, 500, -3.1415/2}; @@ -69,36 +69,16 @@ TCPServer::TCPServer(int port) std::cout << "Server started on port " << port << std::endl; - clients.reserve(7); - ClientTCP tirette; - tirette.name = "tirette"; - ClientTCP aruco; - aruco.name = "aruco"; + clients.emplace_back("tirette"); + clients.emplace_back("aruco"); + clients.emplace_back("ihm"); + clients.emplace_back("lidar"); + clients.emplace_back("arduino"); + clients.emplace_back("servo_moteur"); + clients.emplace_back("point"); - ClientTCP ihm; - ihm.name = "ihm"; - - ClientTCP lidar; - lidar.name = "lidar"; - - ClientTCP arduino; - arduino.name = "arduino"; - - ClientTCP servo_moteur; - servo_moteur.name = "servo_moteur"; - - ClientTCP point; - point.name = "point"; - - clients.push_back(tirette); - clients.push_back(aruco); - clients.push_back(ihm); - clients.push_back(lidar); - clients.push_back(arduino); - clients.push_back(servo_moteur); - clients.push_back(point); } void TCPServer::acceptConnections() @@ -146,8 +126,14 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) // EMERGENCY if (tokens[2] == "stop proximity") { - // TODO handle emergency - // this->broadcastMessage("strat;arduino;stop;1"); + this->stopEmergency = true; + this->broadcastMessage("strat;arduino;clear;1"); + + std::vector args = TCPUtils::split(tokens[3], ","); + + if (!handleEmergencyFlag) { + std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1])); }).detach(); + } } else if (tokens[0] == "tirette" && tokens[2] == "set state") @@ -285,6 +271,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) void TCPServer::broadcastMessage(const char* message, int senderSocket) { + if (stopEmergency) std::terminate(); + for (int clientSocket : clientSockets) { if (clientSocket != senderSocket) { // Exclude the sender's socket send(clientSocket, message, strlen(message), 0); @@ -379,378 +367,94 @@ void TCPServer::checkIfAllClientsReady() } } -void TCPServer::startGameBlueTeam() { - this->setSpeed(130); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); - usleep(100'000); - std::string toSend; - this->go(380, this->robotPose.pos.y); - awaitRobotIdle(); +void TCPServer::startGame() { + for (int i = whereAmI; i < stratPatterns.size(); i++) { + if (stopEmergency) std::terminate(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); - this->go(474, this->robotPose.pos.y); - awaitRobotIdle(); - - this->rotate(0); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); - usleep(100'000); - - this->go(620, this->robotPose.pos.y); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); - this->go(749, this->robotPose.pos.y); - awaitRobotIdle(); - - this->rotate(0); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); - usleep(100'000); - - this->go(1000, 1700); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); - - this->rotate(PI / 2); - awaitRobotIdle(); - this->setSpeed(200); - - this->go(1000, 1800); - awaitRobotIdle(); - - this->rotate(PI / 2); - - usleep(1'000'000); - - this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - - std::optional tag = getBiggestArucoTag(300, 700, -200, 200); - - if (tag.has_value()) { - if (pinceState[1] == NONE) { - goToAruco(tag.value(), 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag.value(), 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag.value(), 0); + switch (stratPatterns[i]) { + case TURN_SOLAR_PANNEL_1: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); + break; + case TURN_SOLAR_PANNEL_2: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_2); + break; + case TURN_SOLAR_PANNEL_3: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_3); + break; + case TAKE_FLOWER_TOP: + findAndGoFlower(TAKE_FLOWER_TOP); + break; + case TAKE_FLOWER_BOTTOM: + findAndGoFlower(TAKE_FLOWER_BOTTOM); + break; + case GO_END: + goEnd(); + break; + case DROP_FLOWER: + dropFlowers(); + break; } } - - this->go(1000, 1700); - awaitRobotIdle(); - - this->rotate(PI / 2); - awaitRobotIdle(); - - usleep(1'000'000); - - this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - - tag = getBiggestArucoTag(300, 700, -200, 200); - - if (tag.has_value()) { - if (pinceState[1] == NONE) { - goToAruco(tag.value(), 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag.value(), 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag.value(), 0); - } - } - - this->go(1000, 1700); - awaitRobotIdle(); - - this->rotate(PI / 2); - awaitRobotIdle(); - - usleep(1'000'000); - - this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - - tag = getBiggestArucoTag(300, 700, -200, 200); - - if (tag.has_value()) { - if (pinceState[1] == NONE) { - goToAruco(tag.value(), 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag.value(), 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag.value(), 0); - } - } - - this->go(1000, 1700); - awaitRobotIdle(); - - this->rotate(PI); - awaitRobotIdle(); - - usleep(1'000'000); - - this->go(this->robotPose.pos.x - 350, this->robotPose.pos.y); - awaitRobotIdle(); - - this->go(500, 500); - awaitRobotIdle(); - - std::vector pinceHavePurpleFlower; - for (int i = 0; i < 3; i++) { - if (pinceState[i] == PURPLE_FLOWER) { - pinceHavePurpleFlower.push_back(i); - } - } - - if (!pinceHavePurpleFlower.empty()) { - this->go(300, 300); - awaitRobotIdle(); - this->rotate(PI / 2); - awaitRobotIdle(); - - for (auto & toDrop : pinceHavePurpleFlower) { - toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(toDrop) + "\n"; - this->broadcastMessage(toSend); - - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); - awaitRobotIdle(); - - pinceState[toDrop] = NONE; - toSend = "strat;servo_moteur;fermer pince;" + std::to_string(toDrop) + "\n"; - this->broadcastMessage(toSend); - usleep(100'000); - } - - this->go(this->robotPose.pos.x + 150, this->robotPose.pos.y); - awaitRobotIdle(); - } - - - if (pinceHavePurpleFlower.size() < 3) { - this->go(762, 300); - awaitRobotIdle(); - - this->setSpeed(130); - this->rotate(PI / 2); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); - - this->go(762, 0); - usleep(3'000'000); - - for (int i = 0; i < 3; i++) { - if (pinceState[i] == WHITE_FLOWER) { - toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(i) + "\n"; - this->broadcastMessage(toSend); - usleep(500'000); - pinceState[i] = NONE; - toSend = "strat;servo_moteur;fermer pince;" + std::to_string(i) + "\n"; - this->broadcastMessage(toSend); - usleep(100'000); - } - } - - this->setSpeed(200); - } - - /* - * TODO - * here add the whole strategy, for the moment the robot only took 3 plants and drop them - * Handle the second plant spot - */ - - this->go(1000, 250); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); - - this->rotate(-PI/4); - awaitRobotIdle(); - - this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - - tag = getBiggestArucoTag(300, 700, -200, 200); - - if (tag.has_value()) { - if (pinceState[1] == NONE) { - goToAruco(tag.value(), 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag.value(), 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag.value(), 0); - } - } - - this->go(1000, 250); - awaitRobotIdle(); - - this->rotate(-PI / 2); - awaitRobotIdle(); - - this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - - tag = getBiggestArucoTag(300, 700, -200, 200); - - if (tag.has_value()) { - if (pinceState[1] == NONE) { - goToAruco(tag.value(), 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag.value(), 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag.value(), 0); - } - } - - - this->go(1000, 250); - awaitRobotIdle(); - - this->rotate(PI / 2); - awaitRobotIdle(); - - this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - - tag = getBiggestArucoTag(300, 700, -200, 200); - - if (tag.has_value()) { - if (pinceState[1] == NONE) { - goToAruco(tag.value(), 1); - } else if (pinceState[2] == NONE) { - goToAruco(tag.value(), 2); - } else if (pinceState[0] == NONE) { - goToAruco(tag.value(), 0); - } - } - - this->go(1000, 250); - awaitRobotIdle(); - - this->rotate(PI); - awaitRobotIdle(); - - pinceHavePurpleFlower.clear(); - for (int i = 0; i < 3; i++) { - if (pinceState[i] == PURPLE_FLOWER) { - pinceHavePurpleFlower.push_back(i); - } - } - - if (!pinceHavePurpleFlower.empty()) { - this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); - - this->go(225, 225); - awaitRobotIdle(); - this->rotate(PI); - awaitRobotIdle(); - - for (auto & toDrop : pinceHavePurpleFlower) { - toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(toDrop) + "\n"; - this->broadcastMessage(toSend); - usleep(500'000); - pinceState[toDrop] = NONE; - toSend = "strat;servo_moteur;fermer pince;" + std::to_string(toDrop) + "\n"; - this->broadcastMessage(toSend); - usleep(100'000); - } - - this->go(this->robotPose.pos.x + 150, this->robotPose.pos.y); - awaitRobotIdle(); - } - - - if (pinceHavePurpleFlower.size() < 3) { - this->go(762, 300); - awaitRobotIdle(); - - this->rotate(PI / 2); - awaitRobotIdle(); - - this->setSpeed(150); - this->go(762, 0); - usleep(1'000'000); - - - for (int i = 0; i < 3; i++) { - if (pinceState[i] == WHITE_FLOWER) { - toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(i) + "\n"; - this->broadcastMessage(toSend); - usleep(200'000); - pinceState[i] = NONE; - toSend = "strat;servo_moteur;fermer pince;" + std::to_string(i) + "\n"; - this->broadcastMessage(toSend); - usleep(100'000); - } - - this->setSpeed(200); - - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 350); - awaitRobotIdle(); - } - } - - - this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); - awaitRobotIdle(); - - this->rotate(this->endRobotPose.theta); - awaitRobotIdle(); - - this->broadcastMessage("strat;servo_moteur;clear;1"); } +void TCPServer::startGameBlueTeam() { + for (int i = whereAmI; i < stratPatterns.size(); i++) { + if (stopEmergency) std::terminate(); + + switch (stratPatterns[i]) { + case TURN_SOLAR_PANNEL_1: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); + break; + case TURN_SOLAR_PANNEL_2: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_2); + break; + case TURN_SOLAR_PANNEL_3: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_3); + break; + case TAKE_FLOWER_TOP: + findAndGoFlower(TAKE_FLOWER_TOP); + break; + case TAKE_FLOWER_BOTTOM: + findAndGoFlower(TAKE_FLOWER_BOTTOM); + break; + case GO_END: + goEnd(); + break; + case DROP_FLOWER: + dropFlowers(); + break; + } + } +} void TCPServer::startGameYellowTeam() { - this->broadcastMessage("strat;arduino;speed;200\n"); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); - usleep(100'000); - std::string toSend = "strat;arduino;go;"; - this->broadcastMessage(toSend); + for (int i = whereAmI; i < stratPatterns.size(); i++) { + if (stopEmergency) std::terminate(); - - 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); - awaitRobotIdle(); - toSend = "strat;arduino;angle;" + std::to_string(static_cast(this->endRobotPose.theta * 100)); - this->broadcastMessage("strat;arduino;speed;150\n"); - this->broadcastMessage(toSend); - awaitRobotIdle(); - this->broadcastMessage("strat;arduino;speed;200\n"); - - this->broadcastMessage("strat;servo_moteur;clear;1"); + switch (stratPatterns[i]) { + case TURN_SOLAR_PANNEL_1: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); + break; + case TURN_SOLAR_PANNEL_2: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_2); + break; + case TURN_SOLAR_PANNEL_3: + goAndTurnSolarPannel(TURN_SOLAR_PANNEL_3); + break; + case TAKE_FLOWER_TOP: + findAndGoFlower(TAKE_FLOWER_TOP); + break; + case TAKE_FLOWER_BOTTOM: + findAndGoFlower(TAKE_FLOWER_BOTTOM); + break; + case GO_END: + goEnd(); + break; + case DROP_FLOWER: + dropFlowers(); + break; + } + } } void TCPServer::startGameTest() { @@ -758,7 +462,6 @@ void TCPServer::startGameTest() { 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"); arucoTags.clear(); @@ -957,10 +660,8 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { break; } - this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); - std::string toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n"; - - this->broadcastMessage(toSend); + this->baisserBras(); + this->openPince(pince); double xPrime = arucoTag.pos()[0]; double yPrime = arucoTag.pos()[1]; @@ -982,8 +683,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { this->transit(robotPosForPotX, robotPosForPotY, 130); awaitRobotIdle(); - toSend = "strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n"; - this->broadcastMessage(toSend); + this->closePince(pince); usleep(500'000); this->setSpeed(previousSpeed); pinceState[pince] = TCPUtils::startWith(arucoTag.name(), "Purple_flower") ? PURPLE_FLOWER : WHITE_FLOWER; @@ -1015,6 +715,8 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAEndlessLoop usleep(200'000); while (isRobotIdle < 3) { + if (stopEmergency) std::terminate(); + usleep(200'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; @@ -1060,8 +762,23 @@ std::optional TCPServer::getBiggestArucoTag(float borneMinX, float bor return found ? std::optional(biggestTag) : std::nullopt; } -void TCPServer::startTestAruco(int pince) { +void TCPServer::handleEmergency(int distance, double angle) { + this->handleEmergencyFlag = true; + // TODO handle here the emergency like wait for 2 second and then if emergency is again on, that means the robot of the other team do not move, if that go back otherwise continue + usleep(2'000'000); + this->stopEmergency = false; + usleep(500'000); + if (this->stopEmergency) { + // TODO here go back by twenty centimeter + } + + this->startGame(); + + this->handleEmergencyFlag = false; +} + +void TCPServer::startTestAruco(int pince) { this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); for (int i = 0; i < 5; i++) { @@ -1076,11 +793,271 @@ void TCPServer::startTestAruco(int pince) { } } +void TCPServer::goEnd() { + this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); + awaitRobotIdle(); + this->rotate(this->endRobotPose.theta); + awaitRobotIdle(); +} + +void TCPServer::findAndGoFlower(StratPattern sp) { + if (team == BLUE) { + if (sp == TAKE_FLOWER_TOP) { + this->go(1000, 250); + awaitRobotIdle(); + + this->rotate(-PI/2); + awaitRobotIdle(); + } + else if (sp == TAKE_FLOWER_BOTTOM) { + this->go(1000, 1800); + awaitRobotIdle(); + + this->rotate(PI / 2); + awaitRobotIdle(); + } else { + return; + } + } else if (team == YELLOW) { + if (sp == TAKE_FLOWER_TOP) { + this->go(2000, 250); + awaitRobotIdle(); + + this->rotate(-PI/2); + awaitRobotIdle(); + } + else if (sp == TAKE_FLOWER_BOTTOM) { + this->go(2000, 1800); + awaitRobotIdle(); + + this->rotate(PI / 2); + awaitRobotIdle(); + } else { + return; + } + } + + this->arucoTags.clear(); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + for (int i = 0; i < 5; i++) { + if (stopEmergency) std::terminate(); + + usleep(200'000); + this->broadcastMessage("strat;aruco;get aruco;1\n"); + } + usleep(100'000); + + std::optional tag = getBiggestArucoTag(300, 700, -200, 200); + + if (tag.has_value()) { + if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); + } + } +} + +void TCPServer::dropFlowers() { + std::vector pinceHavePurpleFlower; + for (int i = 0; i < 3; i++) { + if (pinceState[i] == PURPLE_FLOWER) { + pinceHavePurpleFlower.push_back(i); + } + } + + std::array purpleDrop{}; + std::array whiteDrop{}; + if (team == BLUE) { + purpleDrop = {300, 400}; + whiteDrop = {762, 300}; + } else if (team == YELLOW) { + purpleDrop = {2700, 400}; + whiteDrop = {2237, 400}; + } + + if (!pinceHavePurpleFlower.empty()) { + this->go(purpleDrop); + awaitRobotIdle(); + this->rotate(PI / 2); + awaitRobotIdle(); + + this->leverBras(); + + for (auto & toDrop : pinceHavePurpleFlower) { + this->openPince(toDrop); + usleep(500'000); + + pinceState[toDrop] = NONE; + this->closePince(toDrop); + usleep(100'000); + } + + for (int i = 0; i < 3; i++) { + this->middlePince(i); + } + + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); + awaitRobotIdle(); + + this->baisserBras(); + } + + if (pinceHavePurpleFlower.size() < 3) { + this->go(whiteDrop); + awaitRobotIdle(); + + this->rotate(PI / 2); + awaitRobotIdle(); + + this->leverBras(); + usleep(1'000'000); + + this->setSpeed(130); + + this->go(whiteDrop[0], 0); + usleep(1'000'000); + + for (int i = 0; i < 3; i++) { + if (pinceState[i] == WHITE_FLOWER) { + this->openPince(i); + usleep(500'000); + + pinceState[i] = NONE; + this->closePince(i); + usleep(100'000); + } + } + + for (int i = 0; i < 3; i++) { + this->middlePince(i); + } + + this->setSpeed(200); + } + + this->go(whiteDrop); + awaitRobotIdle(); + + this->baisserBras(); +} + +void TCPServer::goAndTurnSolarPannel(StratPattern sp) { + if (team == BLUE) { + switch (sp) { + case TURN_SOLAR_PANNEL_1: + this->go(250, 1790); + awaitRobotIdle(); + + this->rotate(PI / 2); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + usleep(100'000); + + this->go(380, 1790); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + break; + case TURN_SOLAR_PANNEL_2: + this->go(475, 1790); + awaitRobotIdle(); + + this->rotate(PI / 2); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + usleep(100'000); + + this->go(605, 1790); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + break; + case TURN_SOLAR_PANNEL_3: + this->go(700, 1790); + awaitRobotIdle(); + + this->rotate(PI / 2); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + usleep(100'000); + + this->go(830, 1790); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + break; + default: + break; + } + } else if (team == YELLOW) { + switch (sp) { + case TURN_SOLAR_PANNEL_1: + this->go(2750, 1790); + awaitRobotIdle(); + + this->rotate(-PI / 2); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + usleep(100'000); + + this->go(2620, 1790); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + break; + case TURN_SOLAR_PANNEL_2: + this->go(2525, 1790); + awaitRobotIdle(); + + this->rotate(-PI / 2); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + usleep(100'000); + + this->go(2395, 1790); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + break; + case TURN_SOLAR_PANNEL_3: + this->go(2300, 1790); + awaitRobotIdle(); + + this->rotate(-PI / 2); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + usleep(100'000); + + this->go(2170, 1790); + awaitRobotIdle(); + + this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + break; + default: + break; + } + } +} + template void TCPServer::go(X x, Y y) { 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) { + 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) { this->broadcastMessage("strat;arduino;angle" + std::to_string(static_cast(angle * 100)) + "\n"); @@ -1095,3 +1072,32 @@ template void TCPServer::transit(X x, Y y, int endSpeed) { 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, int endSpeed) { + this->broadcastMessage("strat;arduino;transit" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(endSpeed) + "\n"); +} + +void TCPServer::baisserBras() { + this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); +} + +void TCPServer::transportBras() { + this->broadcastMessage("strat;servo_moteur;transport bras;1\n"); +} + +void TCPServer::leverBras() { + this->broadcastMessage("strat;servo_moteur;lever bras;1\n"); +} + +void TCPServer::openPince(int pince) { + this->broadcastMessage("strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n"); +} + +void TCPServer::middlePince(int pince) { + this->broadcastMessage("strat;servo_moteur;middle pince;" + std::to_string(pince) + "\n"); +} + +void TCPServer::closePince(int pince) { + this->broadcastMessage("strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n"); +} diff --git a/TCPServer.h b/TCPServer.h index 4d7e491..9717d33 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +22,10 @@ struct ClientTCP std::string name; int socket = -1; bool isReady = false; + + ClientTCP() = default; + + explicit ClientTCP(std::string name, int socket = -1) : name(std::move(name)), socket(socket) {} }; enum Team { @@ -29,6 +34,16 @@ enum Team { TEST }; +enum StratPattern { + TURN_SOLAR_PANNEL_1, + TURN_SOLAR_PANNEL_2, + TURN_SOLAR_PANNEL_3, + TAKE_FLOWER_BOTTOM, + TAKE_FLOWER_TOP, + DROP_FLOWER, + GO_END, +}; + class TCPServer; // Forward declaration class ClientHandler { @@ -55,7 +70,7 @@ private: std::atomic _shouldStop = false; // Flag to indicate if the server should stop std::vector clients; // Store connected clients - PinceState pinceState[3] = {NONE, NONE, NONE}; + std::array pinceState = {NONE, NONE, NONE}; int isRobotIdle = 0; int speed = 0; @@ -66,8 +81,9 @@ private: float y; } pos; float theta; - } robotPose{}; + }; + Position robotPose{}; Position initRobotPose{}; Position endRobotPose{}; @@ -75,6 +91,27 @@ private: Team team; + std::vector stratPatterns = { + TURN_SOLAR_PANNEL_1, + TURN_SOLAR_PANNEL_2, + TURN_SOLAR_PANNEL_3, + TAKE_FLOWER_BOTTOM, + TAKE_FLOWER_BOTTOM, + TAKE_FLOWER_BOTTOM, + DROP_FLOWER, + TAKE_FLOWER_TOP, + TAKE_FLOWER_TOP, + TAKE_FLOWER_TOP, + DROP_FLOWER, + GO_END + }; + + // This is the index of the current pattern + int whereAmI = 0; + + bool stopEmergency = false; + bool handleEmergencyFlag = false; + public: explicit TCPServer(int port); @@ -102,6 +139,8 @@ public: void checkIfAllClientsReady(); + void startGame(); + void startGameBlueTeam(); void startGameYellowTeam(); @@ -120,6 +159,22 @@ public: std::optional getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); + void handleEmergency(int distance, double angle); + + /* + * Start Strategy function + */ + void goAndTurnSolarPannel(StratPattern sp); + + void findAndGoFlower(StratPattern sp); + + void goEnd(); + + void dropFlowers(); + /* + * End Strategy function + */ + void startTestAruco(int pince); // Call to broadcast @@ -128,11 +183,29 @@ public: template void go(X x, Y y); + template + void go(std::array data); + template void rotate(X angle); template void transit(X x,Y y, int endSpeed); + template + void transit(std::array data, int endSpeed); + + void openPince(int pince); + + void middlePince(int pince); + + void closePince(int pince); + + void baisserBras(); + + void leverBras(); + + void transportBras(); + ~TCPServer(); }; diff --git a/main.cpp b/main.cpp index 8d02ba5..e5b6443 100644 --- a/main.cpp +++ b/main.cpp @@ -4,10 +4,7 @@ std::atomic shouldStop = false; void signalHandler( int signum ) { - shouldStop = true; - - exit(signum); } int main(int argc, char* argv[]) { diff --git a/utils.cpp b/utils.cpp index a7d1581..51beefc 100644 --- a/utils.cpp +++ b/utils.cpp @@ -87,19 +87,3 @@ void ArucoTag::find() { int ArucoTag::getNbFind() const { return nbFind; } - -FlowerAruco::FlowerAruco() : tag(nullptr), _realPos({0, 0}) { - -} - -FlowerAruco::FlowerAruco(ArucoTag *tag) : tag(tag), _realPos({0, 0}) { - -} - -ArucoTag *FlowerAruco::getTag() const { - return tag; -} - -std::array FlowerAruco::getPos() const { - return _realPos; -} diff --git a/utils.h b/utils.h index d51762d..8ef6309 100644 --- a/utils.h +++ b/utils.h @@ -26,7 +26,7 @@ namespace TCPUtils { class ArucoTag { public: - ArucoTag(int id, std::string name, std::array pos, std::array rot); + ArucoTag(int id, std::string name, std::array pos, std::array rot); ArucoTag() = default; @@ -55,24 +55,7 @@ public: private: int _id = -1; std::string _name; - std::array _pos; - std::array _rot; + std::array _pos = {0, 0}; + std::array _rot = {0, 0, 0}; int nbFind = 0; }; - -class FlowerAruco { - -public: - FlowerAruco(); - - explicit FlowerAruco(ArucoTag* tag); - - [[nodiscard]] ArucoTag* getTag() const; - - [[nodiscard]] std::array getPos() const; - -private: - ArucoTag* tag; - - std::array _realPos; -}; \ No newline at end of file From 95b3d5076eb69bdc62e0eac661155a556d184a4e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 25 Apr 2024 11:32:05 +0200 Subject: [PATCH 002/316] Add ostream for ArucoTag --- utils.cpp | 5 +++++ utils.h | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/utils.cpp b/utils.cpp index 51beefc..173fc0f 100644 --- a/utils.cpp +++ b/utils.cpp @@ -87,3 +87,8 @@ void ArucoTag::find() { int ArucoTag::getNbFind() const { return nbFind; } + +std::ostream& operator<<(std::ostream& os, const ArucoTag& tag) { + os << "ArucoTag{id=" << tag.id() << ", name=" << tag.name() << ", pos=[" << tag.pos()[0] << ", " << tag.pos()[1] << "], rot=[" << tag.rot()[0] << ", " << tag.rot()[1] << ", " << tag.rot()[2] << "]}"; + return os; +} diff --git a/utils.h b/utils.h index 8ef6309..28ad67e 100644 --- a/utils.h +++ b/utils.h @@ -4,6 +4,7 @@ #include #include #include +#include #define PI 3.14159265358979323846 @@ -30,6 +31,8 @@ public: ArucoTag() = default; + ArucoTag(const ArucoTag& other) = default; + [[nodiscard]] int id() const; [[nodiscard]] std::string name() const; @@ -52,6 +55,8 @@ public: [[nodiscard]] int getNbFind() const; + friend std::ostream& operator<<(std::ostream& os, const ArucoTag& tag); + private: int _id = -1; std::string _name; From 0186315d21d558982a85fc274092cb5d663c3d82 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 25 Apr 2024 11:32:29 +0200 Subject: [PATCH 003/316] boolean to know if the game is started --- TCPServer.cpp | 98 ++++++++++++++++++++++++++++++++++++--------------- TCPServer.h | 6 ++++ 2 files changed, 75 insertions(+), 29 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a157ed3..e8c1fd9 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -126,6 +126,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) // EMERGENCY if (tokens[2] == "stop proximity") { + if (!gameStarted) return; + this->stopEmergency = true; this->broadcastMessage("strat;arduino;clear;1"); @@ -223,10 +225,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) switch (this->team) { case BLUE: - std::thread([this]() { this->startGameBlueTeam(); }).detach(); - break; case YELLOW: - std::thread([this]() { this->startGameYellowTeam(); }).detach(); + this->gameStarted = true; + std::thread([this]() { this->startGame(); }).detach(); break; case TEST: std::thread([this]() { this->startGameTest(); }).detach(); @@ -245,6 +246,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) tag.setPos(std::stof(aruco[i + 2]), std::stof(aruco[i + 3])); tag.setRot(std::stof(aruco[i + 4]), std::stof(aruco[i + 5]), std::stof(aruco[i + 6])); + std::cout << tag << std::endl; + handleArucoTag(tag); } // Broadcast the aruco tag to all clients @@ -394,6 +397,7 @@ void TCPServer::startGame() { dropFlowers(); break; } + whereAmI++; } } @@ -749,7 +753,6 @@ void TCPServer::handleArucoTag(ArucoTag &tag) { std::optional TCPServer::getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY) { - // use optional bool found = false; ArucoTag biggestTag = ArucoTag(); for (const auto & tag : arucoTags) { @@ -835,6 +838,8 @@ void TCPServer::findAndGoFlower(StratPattern sp) { } else { return; } + } else { + return; } this->arucoTags.clear(); @@ -862,24 +867,51 @@ void TCPServer::findAndGoFlower(StratPattern sp) { void TCPServer::dropFlowers() { std::vector pinceHavePurpleFlower; + std::vector pinceHaveWhiteFlower; + + std::vector> checkpoints; + + pinceHavePurpleFlower.reserve(3); + pinceHaveWhiteFlower.reserve(3); + for (int i = 0; i < 3; i++) { - if (pinceState[i] == PURPLE_FLOWER) { - pinceHavePurpleFlower.push_back(i); + switch (pinceState[i]) { + case PURPLE_FLOWER: + pinceHavePurpleFlower.push_back(i); + break; + case WHITE_FLOWER: + pinceHaveWhiteFlower.push_back(i); + break; + case NONE: + break; } } - std::array purpleDrop{}; - std::array whiteDrop{}; + std::array purpleDropPosition{}; + std::array whiteDropPosition{}; if (team == BLUE) { - purpleDrop = {300, 400}; - whiteDrop = {762, 300}; + purpleDropPosition = {300, 400}; + whiteDropPosition = {762, 300}; + if (this->robotPose.pos.y > 1000) { + checkpoints.emplace_back(std::array{500, 1700}); + checkpoints.emplace_back(std::array{500, 500}); + } } else if (team == YELLOW) { - purpleDrop = {2700, 400}; - whiteDrop = {2237, 400}; + purpleDropPosition = {2700, 400}; + whiteDropPosition = {2237, 400}; + if (this->robotPose.pos.y > 1000) { + checkpoints.emplace_back(std::array{2500, 1700}); + checkpoints.emplace_back(std::array{2500, 500}); + } + } + + for (const auto& checkpoint : checkpoints) { + this->go(checkpoint); + awaitRobotIdle(); } if (!pinceHavePurpleFlower.empty()) { - this->go(purpleDrop); + this->go(purpleDropPosition); awaitRobotIdle(); this->rotate(PI / 2); awaitRobotIdle(); @@ -905,8 +937,8 @@ void TCPServer::dropFlowers() { this->baisserBras(); } - if (pinceHavePurpleFlower.size() < 3) { - this->go(whiteDrop); + if (!pinceHaveWhiteFlower.empty()) { + this->go(whiteDropPosition); awaitRobotIdle(); this->rotate(PI / 2); @@ -917,7 +949,7 @@ void TCPServer::dropFlowers() { this->setSpeed(130); - this->go(whiteDrop[0], 0); + this->go(whiteDropPosition[0], 0); usleep(1'000'000); for (int i = 0; i < 3; i++) { @@ -938,7 +970,7 @@ void TCPServer::dropFlowers() { this->setSpeed(200); } - this->go(whiteDrop); + this->go(whiteDropPosition); awaitRobotIdle(); this->baisserBras(); @@ -954,13 +986,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + this->checkPanneau(7); usleep(100'000); this->go(380, 1790); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: this->go(475, 1790); @@ -969,13 +1001,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + this->checkPanneau(7); usleep(100'000); this->go(605, 1790); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: this->go(700, 1790); @@ -984,13 +1016,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + this->checkPanneau(7); usleep(100'000); this->go(830, 1790); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->uncheckPanneau(7); break; default: break; @@ -1004,13 +1036,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(-PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + this->checkPanneau(8); usleep(100'000); this->go(2620, 1790); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->uncheckPanneau(8); break; case TURN_SOLAR_PANNEL_2: this->go(2525, 1790); @@ -1019,13 +1051,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(-PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + this->checkPanneau(8); usleep(100'000); this->go(2395, 1790); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->uncheckPanneau(8); break; case TURN_SOLAR_PANNEL_3: this->go(2300, 1790); @@ -1034,13 +1066,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(-PI / 2); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;check panneau;7\n"); + this->checkPanneau(8); usleep(100'000); this->go(2170, 1790); awaitRobotIdle(); - this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n"); + this->uncheckPanneau(8); break; default: break; @@ -1101,3 +1133,11 @@ void TCPServer::middlePince(int pince) { void TCPServer::closePince(int pince) { this->broadcastMessage("strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n"); } + +void TCPServer::checkPanneau(int servo_moteur) { + this->broadcastMessage("strat;servo_moteur;check panneau;" + std::to_string(servo_moteur) + "\n"); +} + +void TCPServer::uncheckPanneau(int servo_moteur) { + this->broadcastMessage("strat;servo_moteur;uncheck panneau;" + std::to_string(servo_moteur) + "\n"); +} diff --git a/TCPServer.h b/TCPServer.h index 9717d33..a527371 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -73,6 +73,8 @@ private: std::array pinceState = {NONE, NONE, NONE}; int isRobotIdle = 0; + bool gameStarted = false; + int speed = 0; struct Position { @@ -207,5 +209,9 @@ public: void transportBras(); + void checkPanneau(int servo_moteur); + + void uncheckPanneau(int servo_moteur); + ~TCPServer(); }; From 1b99f32902c0965d41f456b8c21bf32daaefbd47 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 25 Apr 2024 14:24:19 +0200 Subject: [PATCH 004/316] speed | 90s match | good ending for the main --- TCPServer.cpp | 35 +++++++++++++++++++++++++---------- TCPServer.h | 7 +++++++ main.cpp | 2 +- 3 files changed, 33 insertions(+), 11 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e8c1fd9..3471e1e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -374,6 +374,12 @@ void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { if (stopEmergency) std::terminate(); + auto time = std::chrono::system_clock::now(); + if (time - gameStart > std::chrono::seconds(85)) { + this->goEnd(); + return; + } + switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); @@ -797,6 +803,7 @@ void TCPServer::startTestAruco(int pince) { } void TCPServer::goEnd() { + // TODO checkpoint this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); awaitRobotIdle(); this->rotate(this->endRobotPose.theta); @@ -804,6 +811,7 @@ void TCPServer::goEnd() { } void TCPServer::findAndGoFlower(StratPattern sp) { + this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { this->go(1000, 250); @@ -910,6 +918,8 @@ void TCPServer::dropFlowers() { awaitRobotIdle(); } + this->setSpeed(200); + if (!pinceHavePurpleFlower.empty()) { this->go(purpleDropPosition); awaitRobotIdle(); @@ -918,19 +928,20 @@ void TCPServer::dropFlowers() { this->leverBras(); + this->setSpeed(150); + for (auto & toDrop : pinceHavePurpleFlower) { this->openPince(toDrop); usleep(500'000); + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 50); + awaitRobotIdle(); + pinceState[toDrop] = NONE; this->closePince(toDrop); usleep(100'000); } - for (int i = 0; i < 3; i++) { - this->middlePince(i); - } - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); awaitRobotIdle(); @@ -945,7 +956,7 @@ void TCPServer::dropFlowers() { awaitRobotIdle(); this->leverBras(); - usleep(1'000'000); + usleep(500'000); this->setSpeed(130); @@ -968,15 +979,17 @@ void TCPServer::dropFlowers() { } this->setSpeed(200); + + this->go(whiteDropPosition); + awaitRobotIdle(); + + this->baisserBras(); } - - this->go(whiteDropPosition); - awaitRobotIdle(); - - this->baisserBras(); } void TCPServer::goAndTurnSolarPannel(StratPattern sp) { + int previousSpeed = this->speed; + this->setSpeed(150); if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: @@ -1078,6 +1091,8 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { break; } } + + this->setSpeed(previousSpeed); } template diff --git a/TCPServer.h b/TCPServer.h index a527371..374543a 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -75,6 +75,8 @@ private: bool gameStarted = false; + std::chrono::time_point gameStart; + int speed = 0; struct Position { @@ -93,6 +95,7 @@ private: Team team; + // TODO drop white flower in other jardienière (if time) std::vector stratPatterns = { TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, @@ -105,6 +108,10 @@ private: TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, DROP_FLOWER, + /* TAKE_FLOWER_TOP, + TAKE_FLOWER_TOP, + TAKE_FLOWER_TOP, + DROP_FLOWER, */ GO_END }; diff --git a/main.cpp b/main.cpp index e5b6443..aace0d2 100644 --- a/main.cpp +++ b/main.cpp @@ -20,7 +20,7 @@ int main(int argc, char* argv[]) { try { server.start(); - while (!server.shouldStop() && !shouldStop) { + while (!server.shouldStop() || !shouldStop) { usleep(500'000); } From 0ff3ca63c4720e196a11fb291524d1c9fa7c59d3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 26 Apr 2024 22:15:50 +0200 Subject: [PATCH 005/316] get position with lidar v0.1 --- TCPServer.cpp | 166 ++++++++++++++++++++++++++------------------------ TCPServer.h | 25 ++++++-- 2 files changed, 109 insertions(+), 82 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3471e1e..f5a1d23 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -155,14 +155,17 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } checkIfAllClientsReady(); } - else if (tokens[2] == "set pos") { - std::vector pos = TCPUtils::split(tokens[3], ","); - this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; - } else if (tokens[2] == "get pos") { - std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100) + "\n"; - this->sendToClient(toSend, clientSocket); + this->setPosition(this->robotPose, clientSocket); } + else if (tokens[0] == "lidar" && tokens[2] == "set pos") { + std::vector args = TCPUtils::split(tokens[3], ","); + this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), std::stof(args[2]) / 100}; + this->setPosition(this->lidarCalculatePos); + this->setPosition(this->lidarCalculatePos); + awaitForLidar = false; + } + else if (tokens[2] == "spawn") { int spawnPointNb = std::stoi(tokens[3]); float spawnPoint[3]; @@ -212,10 +215,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; - std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100) + "\n"; for (int j = 0; j < 3; j++) { - this->broadcastMessage(toSend); + this->setPosition(this->robotPose); usleep(200'000); } } @@ -261,6 +263,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } } else if (tokens[2] == "set speed") { this->speed = std::stoi(tokens[3]); + } else if (tokens[2] == "set pos") { + std::vector pos = TCPUtils::split(tokens[3], ","); + this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; } } else if (tokens[2] == "get speed") { this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); @@ -383,90 +388,33 @@ void TCPServer::startGame() { switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); - break; + break; case TURN_SOLAR_PANNEL_2: goAndTurnSolarPannel(TURN_SOLAR_PANNEL_2); - break; + break; case TURN_SOLAR_PANNEL_3: goAndTurnSolarPannel(TURN_SOLAR_PANNEL_3); - break; + break; case TAKE_FLOWER_TOP: findAndGoFlower(TAKE_FLOWER_TOP); - break; + break; case TAKE_FLOWER_BOTTOM: findAndGoFlower(TAKE_FLOWER_BOTTOM); - break; + break; case GO_END: goEnd(); - break; + break; case DROP_FLOWER: dropFlowers(); - break; + break; + case GET_LIDAR_POS: + getLidarPos(); + break; } whereAmI++; } } -void TCPServer::startGameBlueTeam() { - for (int i = whereAmI; i < stratPatterns.size(); i++) { - if (stopEmergency) std::terminate(); - - switch (stratPatterns[i]) { - case TURN_SOLAR_PANNEL_1: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); - break; - case TURN_SOLAR_PANNEL_2: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_2); - break; - case TURN_SOLAR_PANNEL_3: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_3); - break; - case TAKE_FLOWER_TOP: - findAndGoFlower(TAKE_FLOWER_TOP); - break; - case TAKE_FLOWER_BOTTOM: - findAndGoFlower(TAKE_FLOWER_BOTTOM); - break; - case GO_END: - goEnd(); - break; - case DROP_FLOWER: - dropFlowers(); - break; - } - } -} - -void TCPServer::startGameYellowTeam() { - for (int i = whereAmI; i < stratPatterns.size(); i++) { - if (stopEmergency) std::terminate(); - - switch (stratPatterns[i]) { - case TURN_SOLAR_PANNEL_1: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); - break; - case TURN_SOLAR_PANNEL_2: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_2); - break; - case TURN_SOLAR_PANNEL_3: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_3); - break; - case TAKE_FLOWER_TOP: - findAndGoFlower(TAKE_FLOWER_TOP); - break; - case TAKE_FLOWER_BOTTOM: - findAndGoFlower(TAKE_FLOWER_BOTTOM); - break; - case GO_END: - goEnd(); - break; - case DROP_FLOWER: - dropFlowers(); - break; - } - } -} - void TCPServer::startGameTest() { this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); this->broadcastMessage("strat;servo_moteur;fermer pince;1\n"); @@ -782,9 +730,9 @@ void TCPServer::handleEmergency(int distance, double angle) { // TODO here go back by twenty centimeter } - this->startGame(); - this->handleEmergencyFlag = false; + + this->startGame(); } void TCPServer::startTestAruco(int pince) { @@ -803,7 +751,22 @@ void TCPServer::startTestAruco(int pince) { } void TCPServer::goEnd() { - // TODO checkpoint + if (stopEmergency) std::terminate(); + + std::vector> checkponts; + if (this->robotPose.pos.y > 1000) { + if (team == BLUE) { + checkponts.emplace_back(std::array{500, 1700}); + } else if (team == YELLOW) { + checkponts.emplace_back(std::array{2500, 1700}); + } + } + + for (const auto& checkpoint : checkponts) { + this->go(checkpoint); + awaitRobotIdle(); + } + this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); awaitRobotIdle(); this->rotate(this->endRobotPose.theta); @@ -1095,6 +1058,23 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->setSpeed(previousSpeed); } +void TCPServer::getLidarPos() { + + this->askLidarPosition(); + + awaitForLidar = true; + // ReSharper disable once CppDFAConstantConditions + // ReSharper disable once CppDFAEndlessLoop + while (awaitForLidar) { + usleep(200'000); + } + + // ReSharper disable once CppDFAUnreachableCode + std::cout << lidarCalculatePos.pos.x << " " << lidarCalculatePos.pos.y << " " << lidarCalculatePos.theta << std::endl; + +} + + template void TCPServer::go(X x, Y y) { this->broadcastMessage("strat;arduino;go;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "\n"); @@ -1125,6 +1105,32 @@ void TCPServer::transit(std::array data, int endSpeed) { this->broadcastMessage("strat;arduino;transit" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(endSpeed) + "\n"); } +template +void TCPServer::setPosition(X x, Y y, Z theta, int clientSocket) { + if (clientSocket == -1) { + this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(static_cast(theta * 100)) + "\n"); + } else { + this->sendToClient("strat;all;set pos;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(static_cast(theta * 100)) + "\n", clientSocket); + } +} + +template +void TCPServer::setPosition(std::array data, int clientSocket) { + if (clientSocket == -1) { + this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(static_cast(data[2] * 100)) + "\n"); + } else { + this->sendToClient("strat;all;set pos;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(static_cast(data[2] * 100)) + "\n", clientSocket); + } +} + +void TCPServer::setPosition(Position pos, int clientSocket) { + if (clientSocket == -1) { + this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n"); + } else { + this->sendToClient("strat;all;set pos;" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n", clientSocket); + } +} + void TCPServer::baisserBras() { this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); } @@ -1156,3 +1162,7 @@ void TCPServer::checkPanneau(int servo_moteur) { void TCPServer::uncheckPanneau(int servo_moteur) { this->broadcastMessage("strat;servo_moteur;uncheck panneau;" + std::to_string(servo_moteur) + "\n"); } + +void TCPServer::askLidarPosition() { + this->broadcastMessage("start;lidar;get pos;1\n"); +} diff --git a/TCPServer.h b/TCPServer.h index 374543a..fe2983d 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -42,6 +42,7 @@ enum StratPattern { TAKE_FLOWER_TOP, DROP_FLOWER, GO_END, + GET_LIDAR_POS, }; class TCPServer; // Forward declaration @@ -90,6 +91,7 @@ private: Position robotPose{}; Position initRobotPose{}; Position endRobotPose{}; + Position lidarCalculatePos{}; std::vector arucoTags; @@ -100,14 +102,19 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, + GET_LIDAR_POS, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, + GET_LIDAR_POS, DROP_FLOWER, + GET_LIDAR_POS, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, + GET_LIDAR_POS, DROP_FLOWER, + GET_LIDAR_POS, /* TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, @@ -121,6 +128,8 @@ private: bool stopEmergency = false; bool handleEmergencyFlag = false; + bool awaitForLidar = false; + public: explicit TCPServer(int port); @@ -150,10 +159,6 @@ public: void startGame(); - void startGameBlueTeam(); - - void startGameYellowTeam(); - void startGameTest(); void goToAruco(const ArucoTag &arucoTag, int pince); @@ -180,6 +185,8 @@ public: void goEnd(); void dropFlowers(); + + void getLidarPos(); /* * End Strategy function */ @@ -204,6 +211,14 @@ public: template void transit(std::array data, int endSpeed); + template + void setPosition(X x, Y y, Z theta, int clientSocket = -1); + + void setPosition(Position pos, int clientSocket = -1); + + template + void setPosition(std::array data, int clientSocket = -1); + void openPince(int pince); void middlePince(int pince); @@ -220,5 +235,7 @@ public: void uncheckPanneau(int servo_moteur); + void askLidarPosition(); + ~TCPServer(); }; From 0769d4b668aafbeeb32edce4df68529cca41c898 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 26 Apr 2024 22:42:40 +0200 Subject: [PATCH 006/316] use a global thread for the game --- TCPServer.cpp | 49 +++++++++++++++++++++++++++++-------------------- TCPServer.h | 8 ++++++-- 2 files changed, 35 insertions(+), 22 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f5a1d23..6687bdf 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -129,12 +129,13 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (!gameStarted) return; this->stopEmergency = true; + this->gameThread.~thread(); this->broadcastMessage("strat;arduino;clear;1"); std::vector args = TCPUtils::split(tokens[3], ","); if (!handleEmergencyFlag) { - std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1])); }).detach(); + std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1]) / 100); }).detach(); } } @@ -225,16 +226,19 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) { this->broadcastMessage(message.c_str(), clientSocket); + this->gameStarted = true; + switch (this->team) { case BLUE: case YELLOW: - this->gameStarted = true; - std::thread([this]() { this->startGame(); }).detach(); + this->gameThread = std::thread([this]() { this->startGame(); }); break; case TEST: - std::thread([this]() { this->startGameTest(); }).detach(); + this->gameThread = std::thread([this]() { this->startGameTest(); }); break; } + + this->gameThread.detach(); } else if (tokens[0] == "aruco" && tokens[2] == "get aruco") { std::string arucoResponse = tokens[3]; @@ -266,6 +270,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } else if (tokens[2] == "set pos") { std::vector pos = TCPUtils::split(tokens[3], ","); this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; + this->setPosition(this->robotPose, "lidar"); } } else if (tokens[2] == "get speed") { this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); @@ -279,8 +284,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) void TCPServer::broadcastMessage(const char* message, int senderSocket) { - if (stopEmergency) std::terminate(); - for (int clientSocket : clientSockets) { if (clientSocket != senderSocket) { // Exclude the sender's socket send(clientSocket, message, strlen(message), 0); @@ -339,6 +342,8 @@ void TCPServer::stop() { for (auto& thread : clientThreads) { thread.join(); } + this->gameThread.join(); + // Close the server socket close(serverSocket); } @@ -377,7 +382,6 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { - if (stopEmergency) std::terminate(); auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(85)) { @@ -673,8 +677,6 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAEndlessLoop usleep(200'000); while (isRobotIdle < 3) { - if (stopEmergency) std::terminate(); - usleep(200'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; @@ -728,11 +730,18 @@ void TCPServer::handleEmergency(int distance, double angle) { usleep(500'000); if (this->stopEmergency) { // TODO here go back by twenty centimeter + usleep(200'000); } this->handleEmergencyFlag = false; - this->startGame(); + this->gameThread.~thread(); + + this->gameStarted = false; + + this->gameThread = std::thread([this]() { this->startGame(); }); + + this->gameThread.detach(); } void TCPServer::startTestAruco(int pince) { @@ -751,8 +760,6 @@ void TCPServer::startTestAruco(int pince) { } void TCPServer::goEnd() { - if (stopEmergency) std::terminate(); - std::vector> checkponts; if (this->robotPose.pos.y > 1000) { if (team == BLUE) { @@ -816,8 +823,6 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); for (int i = 0; i < 5; i++) { - if (stopEmergency) std::terminate(); - usleep(200'000); this->broadcastMessage("strat;aruco;get aruco;1\n"); } @@ -1090,23 +1095,23 @@ void TCPServer::rotate(X angle) { this->broadcastMessage("strat;arduino;angle" + std::to_string(static_cast(angle * 100)) + "\n"); } -void TCPServer::setSpeed(int speed) { +void TCPServer::setSpeed(const int speed) { this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n"); this->speed = speed; } template -void TCPServer::transit(X x, Y y, int endSpeed) { +void TCPServer::transit(X x, Y y, const int endSpeed) { 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, int endSpeed) { +void TCPServer::transit(std::array data, const int endSpeed) { this->broadcastMessage("strat;arduino;transit" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(endSpeed) + "\n"); } template -void TCPServer::setPosition(X x, Y y, Z theta, int clientSocket) { +void TCPServer::setPosition(X x, Y y, Z theta, const int clientSocket) { if (clientSocket == -1) { this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(static_cast(theta * 100)) + "\n"); } else { @@ -1115,7 +1120,7 @@ void TCPServer::setPosition(X x, Y y, Z theta, int clientSocket) { } template -void TCPServer::setPosition(std::array data, int clientSocket) { +void TCPServer::setPosition(std::array data, const int clientSocket) { if (clientSocket == -1) { this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(static_cast(data[2] * 100)) + "\n"); } else { @@ -1123,7 +1128,7 @@ void TCPServer::setPosition(std::array data, int clientSocket) { } } -void TCPServer::setPosition(Position pos, int clientSocket) { +void TCPServer::setPosition(const Position pos, const int clientSocket) { if (clientSocket == -1) { this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n"); } else { @@ -1131,6 +1136,10 @@ void TCPServer::setPosition(Position pos, int clientSocket) { } } +void TCPServer::setPosition(const Position pos, const std::string &toSend) { + this->broadcastMessage("strat" + toSend + "set pos" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n"); +} + void TCPServer::baisserBras() { this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); } diff --git a/TCPServer.h b/TCPServer.h index fe2983d..77c6c1d 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -130,6 +130,8 @@ private: bool awaitForLidar = false; + std::thread gameThread; + public: explicit TCPServer(int port); @@ -214,11 +216,13 @@ public: template void setPosition(X x, Y y, Z theta, int clientSocket = -1); - void setPosition(Position pos, int clientSocket = -1); - template void setPosition(std::array data, int clientSocket = -1); + void setPosition(Position pos, int clientSocket = -1); + + void setPosition(Position pos, const std::string &toSend); + void openPince(int pince); void middlePince(int pince); From 35fd2194f85105a58252264f0c971838b7b11406 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 26 Apr 2024 22:49:34 +0200 Subject: [PATCH 007/316] use a global thread for the game --- TCPServer.cpp | 146 +++++++++++++++++++++++++++----------------------- TCPServer.h | 6 +++ 2 files changed, 86 insertions(+), 66 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6687bdf..8643054 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -141,7 +141,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (tokens[0] == "tirette" && tokens[2] == "set state") { - this->broadcastMessage(message.c_str(), clientSocket); + this->broadcastMessage(message, clientSocket); } else if (tokens[2] == "ready") { @@ -159,87 +159,93 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (tokens[2] == "get pos") { this->setPosition(this->robotPose, clientSocket); } + else if (tokens[2] == "get speed") { + this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); + } else if (tokens[0] == "lidar" && tokens[2] == "set pos") { std::vector args = TCPUtils::split(tokens[3], ","); this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), std::stof(args[2]) / 100}; this->setPosition(this->lidarCalculatePos); + usleep(100'000); this->setPosition(this->lidarCalculatePos); awaitForLidar = false; } - else if (tokens[2] == "spawn") { - int spawnPointNb = std::stoi(tokens[3]); - float spawnPoint[3]; - float finishPoint[3]; + else if (tokens[0] == "ihm") { + if (tokens[2] == "spawn") { + int spawnPointNb = std::stoi(tokens[3]); + float spawnPoint[3]; + float finishPoint[3]; - switch (spawnPointNb) { - case 3: - this->team = BLUE; - spawnPoint[0] = 250; - spawnPoint[1] = 1790; - spawnPoint[2] = 0; + switch (spawnPointNb) { + case 3: + this->team = BLUE; + spawnPoint[0] = 250; + spawnPoint[1] = 1790; + spawnPoint[2] = 0; - // For test - finishPoint[0] = 400; - finishPoint[1] = 1790; - finishPoint[2] = 0; + // For test + finishPoint[0] = 400; + finishPoint[1] = 1790; + finishPoint[2] = 0; - /*finishPoint[0] = 400; - finishPoint[1] = 400; - finishPoint[2] = 3.1415;*/ - break; - case 6: - this->team = YELLOW; - spawnPoint[0] = 1750; - spawnPoint[1] = 1790; - spawnPoint[2] = 3.1415; - finishPoint[0] = 2600; - finishPoint[1] = 400; - finishPoint[2] = 0; - break; + /*finishPoint[0] = 400; + finishPoint[1] = 400; + finishPoint[2] = 3.1415;*/ + break; + case 6: + this->team = YELLOW; + spawnPoint[0] = 1750; + spawnPoint[1] = 1790; + spawnPoint[2] = 3.1415; + finishPoint[0] = 2600; + finishPoint[1] = 400; + finishPoint[2] = 0; + break; - default: - this->team = TEST; - spawnPoint[0] = 1200; - spawnPoint[1] = 1800; - spawnPoint[2] = 1.57; - finishPoint[0] = 1200; - finishPoint[1] = 1800; - finishPoint[2] = 1.57; - break; + default: + this->team = TEST; + spawnPoint[0] = 1200; + spawnPoint[1] = 1800; + spawnPoint[2] = 1.57; + finishPoint[0] = 1200; + finishPoint[1] = 1800; + finishPoint[2] = 1.57; + break; + } + + std::ofstream file("end_point.txt"); + file << finishPoint[0] << " " << finishPoint[1]; + file.close(); + + this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; + this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; + this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; + + for (int j = 0; j < 3; j++) { + this->setPosition(this->robotPose); + usleep(200'000); + } } + else if (tokens[1] == "strat" && tokens[2] == "start") + { + this->broadcastMessage(message.c_str(), clientSocket); - std::ofstream file("end_point.txt"); - file << finishPoint[0] << " " << finishPoint[1]; - file.close(); + this->gameStarted = true; - this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; - this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; - this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; + switch (this->team) { + case BLUE: + case YELLOW: + this->gameThread = std::thread([this]() { this->startGame(); }); + break; + case TEST: + this->gameThread = std::thread([this]() { this->startGameTest(); }); + break; + } - for (int j = 0; j < 3; j++) { - this->setPosition(this->robotPose); - usleep(200'000); + this->gameThread.detach(); } } - else if (tokens[1] == "strat" && tokens[2] == "start") - { - this->broadcastMessage(message.c_str(), clientSocket); - - this->gameStarted = true; - - switch (this->team) { - case BLUE: - case YELLOW: - this->gameThread = std::thread([this]() { this->startGame(); }); - break; - case TEST: - this->gameThread = std::thread([this]() { this->startGameTest(); }); - break; - } - - this->gameThread.detach(); - } else if (tokens[0] == "aruco" && tokens[2] == "get aruco") { std::string arucoResponse = tokens[3]; if (arucoResponse != "404") { @@ -272,8 +278,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; this->setPosition(this->robotPose, "lidar"); } - } else if (tokens[2] == "get speed") { - this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); } else if (tokens[2] == "test aruco") { int pince = std::stoi(tokens[3]); @@ -1136,6 +1140,16 @@ void TCPServer::setPosition(const Position pos, const int clientSocket) { } } +template +void TCPServer::setPosition(X x, Y y, Z theta, const std::string &toSend) { + this->broadcastMessage("strat" + toSend + "set pos" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(static_cast(theta * 100)) + "\n"); +} + +template +void TCPServer::setPosition(std::array data, const std::string &toSend) { + this->broadcastMessage("strat" + toSend + "set pos" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(static_cast(data[2] * 100)) + "\n"); +} + void TCPServer::setPosition(const Position pos, const std::string &toSend) { this->broadcastMessage("strat" + toSend + "set pos" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n"); } diff --git a/TCPServer.h b/TCPServer.h index 77c6c1d..a60c360 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -221,6 +221,12 @@ public: void setPosition(Position pos, int clientSocket = -1); + template + void setPosition(X x, Y y, Z theta, const std::string &toSend); + + template + void setPosition(std::array data, const std::string &toSend); + void setPosition(Position pos, const std::string &toSend); void openPince(int pince); From 9530b82b04aac93d165202d56abaf5ec737c1555 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:11:13 +0200 Subject: [PATCH 008/316] don't use point for the moment --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8643054..3158acf 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -77,7 +77,7 @@ TCPServer::TCPServer(int port) : team(TEST) clients.emplace_back("lidar"); clients.emplace_back("arduino"); clients.emplace_back("servo_moteur"); - clients.emplace_back("point"); + // clients.emplace_back("point"); } From 7066f26608af3d3651ffa3aa7fe377a6f60ff0ec Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:15:23 +0200 Subject: [PATCH 009/316] ; --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3158acf..0b58773 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -69,7 +69,7 @@ TCPServer::TCPServer(int port) : team(TEST) std::cout << "Server started on port " << port << std::endl; - clients.reserve(7); + clients.reserve(6); clients.emplace_back("tirette"); clients.emplace_back("aruco"); @@ -1142,16 +1142,16 @@ void TCPServer::setPosition(const Position pos, const int clientSocket) { template void TCPServer::setPosition(X x, Y y, Z theta, const std::string &toSend) { - this->broadcastMessage("strat" + toSend + "set pos" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(static_cast(theta * 100)) + "\n"); + this->broadcastMessage("strat;" + toSend + ";set pos;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(static_cast(theta * 100)) + "\n"); } template void TCPServer::setPosition(std::array data, const std::string &toSend) { - this->broadcastMessage("strat" + toSend + "set pos" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(static_cast(data[2] * 100)) + "\n"); + this->broadcastMessage("strat;" + toSend + ";set pos;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(static_cast(data[2] * 100)) + "\n"); } void TCPServer::setPosition(const Position pos, const std::string &toSend) { - this->broadcastMessage("strat" + toSend + "set pos" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n"); + this->broadcastMessage("strat;" + toSend + ";set pos;" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n"); } void TCPServer::baisserBras() { From cc4652a5dabaa7e21fb438981557cf344b3aaf8f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:17:41 +0200 Subject: [PATCH 010/316] don't use lidar for the moment --- TCPServer.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index a60c360..f04a763 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -102,19 +102,19 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, - GET_LIDAR_POS, + // GET_LIDAR_POS, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, - GET_LIDAR_POS, + // GET_LIDAR_POS, DROP_FLOWER, - GET_LIDAR_POS, + // GET_LIDAR_POS, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, - GET_LIDAR_POS, + // GET_LIDAR_POS, DROP_FLOWER, - GET_LIDAR_POS, + // GET_LIDAR_POS, /* TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, From e5ee5ab861c26d640f27583baecf24d3a63402fe Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:23:33 +0200 Subject: [PATCH 011/316] switch || to && --- main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main.cpp b/main.cpp index aace0d2..e5b6443 100644 --- a/main.cpp +++ b/main.cpp @@ -20,7 +20,7 @@ int main(int argc, char* argv[]) { try { server.start(); - while (!server.shouldStop() || !shouldStop) { + while (!server.shouldStop() && !shouldStop) { usleep(500'000); } From c427feb6aea6717e81b0303ae0b6ae31bb655de5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:27:28 +0200 Subject: [PATCH 012/316] send data only to lidar --- TCPServer.cpp | 5 ++++- TCPServer.h | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0b58773..fd62513 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -151,6 +151,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) { client.isReady = true; client.socket = clientSocket; + if (client.name == "lidar") { + this->lidarSocket = clientSocket; + } break; } } @@ -276,7 +279,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } else if (tokens[2] == "set pos") { std::vector pos = TCPUtils::split(tokens[3], ","); this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; - this->setPosition(this->robotPose, "lidar"); + this->setPosition(this->robotPose, lidarSocket); } } else if (tokens[2] == "test aruco") { int pince = std::stoi(tokens[3]); diff --git a/TCPServer.h b/TCPServer.h index f04a763..4d0055d 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -132,6 +132,8 @@ private: std::thread gameThread; + int lidarSocket = -1; + public: explicit TCPServer(int port); From bd23d101d7285eb9c3e2318b75a9a5d80ba0eefc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:44:16 +0200 Subject: [PATCH 013/316] remove logs --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index fd62513..478e54e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -261,7 +261,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) tag.setPos(std::stof(aruco[i + 2]), std::stof(aruco[i + 3])); tag.setRot(std::stof(aruco[i + 4]), std::stof(aruco[i + 5]), std::stof(aruco[i + 6])); - std::cout << tag << std::endl; + // std::cout << tag << std::endl; handleArucoTag(tag); } @@ -286,7 +286,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::thread([this, pince]() { this->startTestAruco(pince); }).detach(); } - std::cout << "Received: " << message << std::endl; + // std::cout << "Received: " << message << std::endl; } void TCPServer::broadcastMessage(const char* message, int senderSocket) From 2cd930c482eecb5aee028f8562aba459e32cd036 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:46:57 +0200 Subject: [PATCH 014/316] logs --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 478e54e..f2e2b6d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -110,7 +110,7 @@ void TCPServer::acceptConnections() void TCPServer::handleMessage(const std::string& message, int clientSocket) { - // std::cout << message << std::endl; + std::cout << message << std::endl; std::vector tokens = TCPUtils::split(message, ";"); From 521f1e45708a1cb3f39739cf896fafbbc8bb5abe Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:48:21 +0200 Subject: [PATCH 015/316] logs --- TCPServer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f2e2b6d..3024992 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -25,7 +25,7 @@ void ClientHandler::handle() { std::cout << "Client disconnected." << std::endl; break; // Client disconnected } else { - std::cerr << "Failed to receive data." << std::endl; + std::cerr << "Failed to receive data." << this->clientSocket << std::endl; break; // Error in receiving data } } @@ -154,6 +154,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (client.name == "lidar") { this->lidarSocket = clientSocket; } + std::cout << client.socket << " | " << client.name << " is ready" << std::endl; break; } } From 41a92aa353160dc19e110d7555d24ed2f02e5cec Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:55:33 +0200 Subject: [PATCH 016/316] send message --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3024992..625abc0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -112,6 +112,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) { std::cout << message << std::endl; + this->broadcastMessage(message, clientSocket); + std::vector tokens = TCPUtils::split(message, ";"); if (tokens.size() != 4) From b3a43f257bf6a9065801e4895a4b95c720cec206 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 11:57:36 +0200 Subject: [PATCH 017/316] ad client socket --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 625abc0..0e4d0bd 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -112,7 +112,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) { std::cout << message << std::endl; - this->broadcastMessage(message, clientSocket); + this->broadcastMessage(message + std::to_string(clientSocket), clientSocket); std::vector tokens = TCPUtils::split(message, ";"); From aeb6cef8256f61a695d306044aa05ef03520ff89 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:09:33 +0200 Subject: [PATCH 018/316] log --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0e4d0bd..a844341 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -22,7 +22,7 @@ void ClientHandler::handle() { buffer.clear(); } else if (valread == 0) { - std::cout << "Client disconnected." << std::endl; + std::cout << "Client disconnected. " << clientSocket << std::endl; break; // Client disconnected } else { std::cerr << "Failed to receive data." << this->clientSocket << std::endl; From c7a817612353e3d584c6dfb489e0d6dd51d3b369 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:11:02 +0200 Subject: [PATCH 019/316] log --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a844341..519fed5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -112,8 +112,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) { std::cout << message << std::endl; - this->broadcastMessage(message + std::to_string(clientSocket), clientSocket); - std::vector tokens = TCPUtils::split(message, ";"); if (tokens.size() != 4) From 97282ccd9c3aba1e34867d459d49e4120cf16364 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:34:23 +0200 Subject: [PATCH 020/316] ; --- TCPServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 519fed5..1a72ad8 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1100,7 +1100,7 @@ void TCPServer::go(std::array data) { template void TCPServer::rotate(X angle) { - this->broadcastMessage("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) { @@ -1110,12 +1110,12 @@ void TCPServer::setSpeed(const int speed) { template void TCPServer::transit(X x, Y y, const int endSpeed) { - this->broadcastMessage("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) { - this->broadcastMessage("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"); } template From 87a825171d229571a5961bdb9453f17967aaf851 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:41:09 +0200 Subject: [PATCH 021/316] default pos --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 1a72ad8..85bfab2 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -227,7 +227,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; for (int j = 0; j < 3; j++) { - this->setPosition(this->robotPose); + this->setPosition(this->initRobotPose); usleep(200'000); } } From c0d40abd26336f3318b1de0ef5c0e544c749b7a8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:44:28 +0200 Subject: [PATCH 022/316] log --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 85bfab2..99ee947 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -116,7 +116,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (tokens.size() != 4) { - std::cerr << "Invalid message format : " << message << std::endl; + std::cerr << "Invalid message format, token size : " << std::to_string(tokens.size()) << " from message : " << message << std::endl; return; } if (tokens[1] != "strat") From 472887da4674b58986682f117b527017c7b31d5e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:46:54 +0200 Subject: [PATCH 023/316] 2 message at once --- TCPServer.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 99ee947..0d9b16e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -12,13 +12,16 @@ void ClientHandler::handle() { if (valread > 0) { buffer.append(tempBuffer, valread); - //std::cout << "Received: " << buffer << std::endl; if (buffer == "quit") { std::cerr << "Client requested to quit. Closing connection." << std::endl; break; } - processMessage(buffer); + + std::vector messages = TCPUtils::split(buffer, "\n"); + for (const std::string& message : messages) { + processMessage(message); + } buffer.clear(); } else if (valread == 0) { From eee84b4e1948540b755d42453910a5ff8d7e9189 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:55:56 +0200 Subject: [PATCH 024/316] clock --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0d9b16e..fc3a008 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -240,6 +240,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->gameStarted = true; + this->gameStart = std::chrono::system_clock::now(); + switch (this->team) { case BLUE: case YELLOW: From fb22a383e80dd1af48a5c017e682c27a92ecc660 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 12:56:14 +0200 Subject: [PATCH 025/316] speed --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index fc3a008..98c3f18 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -773,6 +773,7 @@ void TCPServer::startTestAruco(int pince) { } void TCPServer::goEnd() { + this->setSpeed(200); std::vector> checkponts; if (this->robotPose.pos.y > 1000) { if (team == BLUE) { From 270237be2955fd4784dee792851fd8cbe9e25f52 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 13:40:12 +0200 Subject: [PATCH 026/316] change angle --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 98c3f18..e4ad862 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -978,7 +978,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(250, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->rotate(-PI / 2); awaitRobotIdle(); this->checkPanneau(7); @@ -993,7 +993,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(475, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->rotate(-PI / 2); awaitRobotIdle(); this->checkPanneau(7); @@ -1008,7 +1008,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(700, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->rotate(-PI / 2); awaitRobotIdle(); this->checkPanneau(7); @@ -1028,7 +1028,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(2750, 1790); awaitRobotIdle(); - this->rotate(-PI / 2); + this->rotate(PI / 2); awaitRobotIdle(); this->checkPanneau(8); @@ -1043,7 +1043,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(2525, 1790); awaitRobotIdle(); - this->rotate(-PI / 2); + this->rotate(PI / 2); awaitRobotIdle(); this->checkPanneau(8); @@ -1058,7 +1058,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(2300, 1790); awaitRobotIdle(); - this->rotate(-PI / 2); + this->rotate(PI / 2); awaitRobotIdle(); this->checkPanneau(8); From 1734ce948cbc4d21c85c66e1a8cfdd5b032e1f02 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 13:47:36 +0200 Subject: [PATCH 027/316] remove the go end for the moment --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e4ad862..7d018d7 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -396,11 +396,11 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { - auto time = std::chrono::system_clock::now(); + /*auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(85)) { this->goEnd(); return; - } + }*/ switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: From 8823d184a55d79f37c56a8c1044a81c9f94a4865 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 13:48:12 +0200 Subject: [PATCH 028/316] sleep / 2 --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7d018d7..dcaf7fe 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -688,8 +688,8 @@ void TCPServer::awaitRobotIdle() { int timeout = 0; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop - usleep(200'000); - while (isRobotIdle < 3) { + usleep(100'000); + while (isRobotIdle < 2) { usleep(200'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; From f1d37d91d8f056bbe0ba17296091f5d41af8b181 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 14:29:37 +0200 Subject: [PATCH 029/316] turn --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index dcaf7fe..3c4adc6 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -978,7 +978,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(250, 1790); awaitRobotIdle(); - this->rotate(-PI / 2); + this->rotate(0); awaitRobotIdle(); this->checkPanneau(7); @@ -993,7 +993,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(475, 1790); awaitRobotIdle(); - this->rotate(-PI / 2); + this->rotate(0); awaitRobotIdle(); this->checkPanneau(7); @@ -1008,7 +1008,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(700, 1790); awaitRobotIdle(); - this->rotate(-PI / 2); + this->rotate(0); awaitRobotIdle(); this->checkPanneau(7); @@ -1028,7 +1028,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(2750, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->rotate(PI); awaitRobotIdle(); this->checkPanneau(8); @@ -1043,7 +1043,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(2525, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->rotate(PI); awaitRobotIdle(); this->checkPanneau(8); @@ -1058,7 +1058,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->go(2300, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->rotate(PI); awaitRobotIdle(); this->checkPanneau(8); From 0c28b4afacd396b3521e8db7d30ff81b4f6d33d3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 15:39:55 +0200 Subject: [PATCH 030/316] \n --- TCPServer.cpp | 32 ++++++++++++++++++++++++-------- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3c4adc6..4ac56d7 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -297,15 +297,29 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) void TCPServer::broadcastMessage(const char* message, int senderSocket) { + std::string temp = std::string(message); + if (temp[temp.size() - 1] != '\n') { + temp += '\n'; + } + for (int clientSocket : clientSockets) { if (clientSocket != senderSocket) { // Exclude the sender's socket - send(clientSocket, message, strlen(message), 0); + send(clientSocket, temp.c_str(), temp.length(), 0); } } } void TCPServer::broadcastMessage(const std::string &message, int senderSocket) { - this->broadcastMessage(message.c_str(), senderSocket); + std::string temp = const_cast(message); + if (temp[temp.size() - 1] != '\n') { + temp += '\n'; + } + + for (int clientSocket : clientSockets) { + if (clientSocket != senderSocket) { // Exclude the sender's socket + send(clientSocket, temp.c_str(), temp.length(), 0); + } + } } void TCPServer::sendToClient(const std::string &message, int clientSocket) { @@ -695,6 +709,7 @@ void TCPServer::awaitRobotIdle() { timeout++; if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); + break; } } } @@ -972,6 +987,7 @@ void TCPServer::dropFlowers() { void TCPServer::goAndTurnSolarPannel(StratPattern sp) { int previousSpeed = this->speed; this->setSpeed(150); + usleep(10'000); if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: @@ -1031,13 +1047,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(8); + this->checkPanneau(6); usleep(100'000); this->go(2620, 1790); awaitRobotIdle(); - this->uncheckPanneau(8); + this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: this->go(2525, 1790); @@ -1046,13 +1062,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(8); + this->checkPanneau(6); usleep(100'000); this->go(2395, 1790); awaitRobotIdle(); - this->uncheckPanneau(8); + this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: this->go(2300, 1790); @@ -1061,13 +1077,13 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(8); + this->checkPanneau(6); usleep(100'000); this->go(2170, 1790); awaitRobotIdle(); - this->uncheckPanneau(8); + this->uncheckPanneau(6); break; default: break; From b6d6142c9e865e9f227f88bee4ff51f755147a8c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 15:55:59 +0200 Subject: [PATCH 031/316] add default speed --- TCPServer.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4ac56d7..64caeb8 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -242,6 +242,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->gameStart = std::chrono::system_clock::now(); + this->broadcastMessage("strat;arduino;speed;200"); + switch (this->team) { case BLUE: case YELLOW: @@ -851,8 +853,8 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(200'000); + for (int i = 0; i < 4; i++) { + usleep(300'000); this->broadcastMessage("strat;aruco;get aruco;1\n"); } usleep(100'000); From 6b1eb21f46c46f83226c9fa454d9f62f012bff26 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 15:56:13 +0200 Subject: [PATCH 032/316] add default speed --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 64caeb8..b9686d1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -242,7 +242,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->gameStart = std::chrono::system_clock::now(); - this->broadcastMessage("strat;arduino;speed;200"); + this->setSpeed(200); switch (this->team) { case BLUE: From dcdfb70fb1119e2206c47e36e31d65cded4e4a96 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 15:57:51 +0200 Subject: [PATCH 033/316] \n things --- TCPServer.cpp | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b9686d1..d49bde4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -124,7 +124,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } if (tokens[1] != "strat") { - this->broadcastMessage(message.c_str(), clientSocket); + this->broadcastMessage(message, clientSocket); } // EMERGENCY @@ -236,7 +236,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } else if (tokens[1] == "strat" && tokens[2] == "start") { - this->broadcastMessage(message.c_str(), clientSocket); + this->broadcastMessage(message, clientSocket); this->gameStarted = true; @@ -274,7 +274,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) handleArucoTag(tag); } // Broadcast the aruco tag to all clients - this->broadcastMessage(message.c_str(), clientSocket); + this->broadcastMessage(message, clientSocket); } } else if (tokens[0] == "arduino") { @@ -325,13 +325,28 @@ void TCPServer::broadcastMessage(const std::string &message, int senderSocket) { } void TCPServer::sendToClient(const std::string &message, int clientSocket) { - this->sendToClient(message.c_str(), clientSocket); + std::string temp = const_cast(message); + if (temp[temp.size() - 1] != '\n') { + temp += '\n'; + } + + for (int socket : clientSockets) { + if (socket == clientSocket) { + send(socket, temp.c_str(), temp.size(), 0); + break; + } + } } void TCPServer::sendToClient(const char *message, int clientSocket) { + std::string temp = std::string(message); + if (temp[temp.size() - 1] != '\n') { + temp += '\n'; + } + for (int socket : clientSockets) { if (socket == clientSocket) { - send(socket, message, strlen(message), 0); + send(socket, temp.c_str(), temp.size(), 0); break; } } From 10d3ba13da958f2c82b47b33268aae0c66bf01ef Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 16:09:49 +0200 Subject: [PATCH 034/316] get aruco tag more time (for test purpose) --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d49bde4..6f5abdf 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -868,8 +868,8 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 4; i++) { - usleep(300'000); + for (int i = 0; i < 5; i++) { + usleep(500'000); this->broadcastMessage("strat;aruco;get aruco;1\n"); } usleep(100'000); From 6f481a92475daa46d8ef461e7d811b13095d6db9 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 16:42:01 +0200 Subject: [PATCH 035/316] reduce distance to tag --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6f5abdf..9682d25 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -673,7 +673,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double yPrime = arucoTag.pos()[1]; double roll = arucoTag.rot()[1]; - auto centerPlantX = (20 * std::cos(roll)) + xPrime/* - 50*/; + auto centerPlantX = (20 * std::cos(roll)) + xPrime - 50; auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); From f82cf53358d0dfb056df0825f1fd6332ff41ff82 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 16:51:37 +0200 Subject: [PATCH 036/316] distance to tag --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9682d25..4540d01 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -673,12 +673,12 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double yPrime = arucoTag.pos()[1]; double roll = arucoTag.rot()[1]; - auto centerPlantX = (20 * std::cos(roll)) + xPrime - 50; + auto centerPlantX = (20 * std::cos(roll)) + xPrime; auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); - this->rotate(this->robotPose.theta + rotate - thetaPrime); + this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); awaitRobotIdle(); double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX; From f374ce669279e9a307a5bc73231db8131cdf7454 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 16:58:40 +0200 Subject: [PATCH 037/316] speed | finish point --- TCPServer.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4540d01..29a85e9 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -192,13 +192,13 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) spawnPoint[2] = 0; // For test - finishPoint[0] = 400; - finishPoint[1] = 1790; - finishPoint[2] = 0; - /*finishPoint[0] = 400; + finishPoint[1] = 1790; + finishPoint[2] = 0;*/ + + finishPoint[0] = 400; finishPoint[1] = 400; - finishPoint[2] = 3.1415;*/ + finishPoint[2] = 3.1415; break; case 6: this->team = YELLOW; @@ -427,11 +427,11 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { - /*auto time = std::chrono::system_clock::now(); + auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(85)) { this->goEnd(); return; - }*/ + } switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: @@ -962,6 +962,8 @@ void TCPServer::dropFlowers() { this->baisserBras(); } + this->setSpeed(200); + if (!pinceHaveWhiteFlower.empty()) { this->go(whiteDropPosition); awaitRobotIdle(); From d0b787b988b6484b1bfad9ae585e938eb4129844 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 17:05:39 +0200 Subject: [PATCH 038/316] acceptable distance --- TCPServer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 29a85e9..4cd6550 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -830,14 +830,14 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { - this->go(1000, 250); + this->go(1000, 300); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1800); + this->go(1000, 1700); awaitRobotIdle(); this->rotate(PI / 2); @@ -847,14 +847,14 @@ void TCPServer::findAndGoFlower(StratPattern sp) { } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { - this->go(2000, 250); + this->go(2000, 300); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1800); + this->go(2000, 1700); awaitRobotIdle(); this->rotate(PI / 2); @@ -874,7 +874,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { } usleep(100'000); - std::optional tag = getBiggestArucoTag(300, 700, -200, 200); + std::optional tag = getBiggestArucoTag(300, 800, -200, 200); if (tag.has_value()) { if (pinceState[1] == NONE) { From d049a63c0c99ab2e5e0e527d09c9599076840be8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 17:09:39 +0200 Subject: [PATCH 039/316] borne --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4cd6550..36a9a71 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -874,7 +874,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { } usleep(100'000); - std::optional tag = getBiggestArucoTag(300, 800, -200, 200); + std::optional tag = getBiggestArucoTag(100, 800, -200, 200); if (tag.has_value()) { if (pinceState[1] == NONE) { From 2965488f174c461c9141cbb3aa331cfb9d5a30ab Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 17:14:01 +0200 Subject: [PATCH 040/316] flower spot --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 36a9a71..4126d28 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -830,14 +830,14 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { - this->go(1000, 300); + this->go(1000, 200); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1700); + this->go(1000, 1800); awaitRobotIdle(); this->rotate(PI / 2); @@ -847,14 +847,14 @@ void TCPServer::findAndGoFlower(StratPattern sp) { } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { - this->go(2000, 300); + this->go(2000, 200); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1700); + this->go(2000, 1800); awaitRobotIdle(); this->rotate(PI / 2); From e36afc0d2a889d5026452e084da2df3da38b9e1e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 17:29:02 +0200 Subject: [PATCH 041/316] end --- TCPServer.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4126d28..7f69759 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -197,7 +197,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) finishPoint[2] = 0;*/ finishPoint[0] = 400; - finishPoint[1] = 400; + finishPoint[1] = 550; finishPoint[2] = 3.1415; break; case 6: @@ -205,8 +205,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) spawnPoint[0] = 1750; spawnPoint[1] = 1790; spawnPoint[2] = 3.1415; + finishPoint[0] = 2600; - finishPoint[1] = 400; + finishPoint[1] = 550; finishPoint[2] = 0; break; @@ -824,20 +825,22 @@ void TCPServer::goEnd() { awaitRobotIdle(); this->rotate(this->endRobotPose.theta); awaitRobotIdle(); + + this->broadcastMessage("strat;all;end;1"); } void TCPServer::findAndGoFlower(StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { - this->go(1000, 200); + this->go(1000, 250); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1800); + this->go(1000, 1750); awaitRobotIdle(); this->rotate(PI / 2); @@ -847,14 +850,14 @@ void TCPServer::findAndGoFlower(StratPattern sp) { } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { - this->go(2000, 200); + this->go(2000, 250); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1800); + this->go(2000, 1750); awaitRobotIdle(); this->rotate(PI / 2); From de669dcbdac8b0eed920cf5460220cafe6525f85 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 17:31:20 +0200 Subject: [PATCH 042/316] timeout --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7f69759..c206179 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -871,8 +871,8 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); + for (int i = 0; i < 3; i++) { + usleep(400'000); this->broadcastMessage("strat;aruco;get aruco;1\n"); } usleep(100'000); From e453717511b2492593bc485dfe052c5479af34fb Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 17:37:00 +0200 Subject: [PATCH 043/316] LET HIM COOK --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c206179..bd66d4f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -198,13 +198,13 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) finishPoint[0] = 400; finishPoint[1] = 550; - finishPoint[2] = 3.1415; + finishPoint[2] = -PI; break; case 6: this->team = YELLOW; spawnPoint[0] = 1750; spawnPoint[1] = 1790; - spawnPoint[2] = 3.1415; + spawnPoint[2] = PI; finishPoint[0] = 2600; finishPoint[1] = 550; @@ -833,14 +833,14 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { - this->go(1000, 250); + this->go(1000, 210); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1750); + this->go(1000, 1790); awaitRobotIdle(); this->rotate(PI / 2); @@ -850,14 +850,14 @@ void TCPServer::findAndGoFlower(StratPattern sp) { } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { - this->go(2000, 250); + this->go(2000, 210); awaitRobotIdle(); this->rotate(-PI/2); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1750); + this->go(2000, 1790); awaitRobotIdle(); this->rotate(PI / 2); From aa43fcd26b44b92c08d6f9549498211dc212d2d3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 18:26:08 +0200 Subject: [PATCH 044/316] finish point --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index bd66d4f..35fc58c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -197,8 +197,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) finishPoint[2] = 0;*/ finishPoint[0] = 400; - finishPoint[1] = 550; - finishPoint[2] = -PI; + finishPoint[1] = 500; + finishPoint[2] = PI / 2; break; case 6: this->team = YELLOW; @@ -207,8 +207,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) spawnPoint[2] = PI; finishPoint[0] = 2600; - finishPoint[1] = 550; - finishPoint[2] = 0; + finishPoint[1] = 500; + finishPoint[2] = PI / 2; break; default: From 7e4d1d76a9853148a8be2ae4e4e79688e134cb06 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 29 Apr 2024 22:45:43 +0200 Subject: [PATCH 045/316] change aruco finder --- TCPServer.cpp | 36 ++++++++++++++++++++++++++---------- TCPServer.h | 2 ++ utils.cpp | 4 ++++ utils.h | 3 +++ 4 files changed, 35 insertions(+), 10 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 35fc58c..e5811a0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -429,7 +429,8 @@ void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { auto time = std::chrono::system_clock::now(); - if (time - gameStart > std::chrono::seconds(85)) { + // TODO check for the bbest timing to return to the spawn + if (time - gameStart > std::chrono::seconds(80)) { this->goEnd(); return; } @@ -643,7 +644,6 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double robotPosY = this->robotPose.pos.y; double theta = this->robotPose.theta; double decalage; - double rotate; if (pince < 0 || pince > 2) { return; } @@ -651,19 +651,15 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { switch (pince) { case 0: decalage = 60; - rotate = -0.07; break; case 1: decalage = 0; - rotate = 0; break; case 2: decalage = -60; - rotate = 0.07; break; default: decalage = 0; - rotate = 0; break; } @@ -767,6 +763,26 @@ std::optional TCPServer::getBiggestArucoTag(float borneMinX, float bor return found ? std::optional(biggestTag) : std::nullopt; } + +std::optional TCPServer::getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY) { + bool found = false; + ArucoTag mostCenteredTag = ArucoTag(); + for (const auto & tag : arucoTags) { + if (tag.getNbFind() < 2) break; + + if (!found) { + if (tag.pos()[0] > borneMinX && tag.pos()[0] < borneMaxX && tag.pos()[1] > borneMinY && tag.pos()[1] < borneMaxY) { + mostCenteredTag = tag; + found = true; + } + } else if (distanceToTag(tag) < distanceToTag(mostCenteredTag)) { + mostCenteredTag = tag; + } + } + + return found ? std::optional(mostCenteredTag) : std::nullopt; +} + void TCPServer::handleEmergency(int distance, double angle) { this->handleEmergencyFlag = true; @@ -871,13 +887,13 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 3; i++) { - usleep(400'000); + for (int i = 0; i < 4; i++) { + usleep(250'000); this->broadcastMessage("strat;aruco;get aruco;1\n"); } usleep(100'000); - std::optional tag = getBiggestArucoTag(100, 800, -200, 200); + std::optional tag = getMostCenteredArucoTag(100, 800, -200, 200); if (tag.has_value()) { if (pinceState[1] == NONE) { @@ -980,7 +996,7 @@ void TCPServer::dropFlowers() { this->setSpeed(130); this->go(whiteDropPosition[0], 0); - usleep(1'000'000); + usleep(2'000'000); for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { diff --git a/TCPServer.h b/TCPServer.h index 4d0055d..ccde6d4 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -177,6 +177,8 @@ public: std::optional getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); + std::optional getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); + void handleEmergency(int distance, double angle); /* diff --git a/utils.cpp b/utils.cpp index 173fc0f..bd1c25c 100644 --- a/utils.cpp +++ b/utils.cpp @@ -92,3 +92,7 @@ std::ostream& operator<<(std::ostream& os, const ArucoTag& tag) { os << "ArucoTag{id=" << tag.id() << ", name=" << tag.name() << ", pos=[" << tag.pos()[0] << ", " << tag.pos()[1] << "], rot=[" << tag.rot()[0] << ", " << tag.rot()[1] << ", " << tag.rot()[2] << "]}"; return os; } + +double distanceToTag(const ArucoTag& tag) { + return std::sqrt(pow(tag.pos()[0], 2) + pow(tag.pos()[1], 2)); +} \ No newline at end of file diff --git a/utils.h b/utils.h index 28ad67e..0563e5a 100644 --- a/utils.h +++ b/utils.h @@ -5,6 +5,7 @@ #include #include #include +#include #define PI 3.14159265358979323846 @@ -64,3 +65,5 @@ private: std::array _rot = {0, 0, 0}; int nbFind = 0; }; + +double distanceToTag(const ArucoTag& tag); \ No newline at end of file From 875b187eabebabd1da6d1464c185a84ef25a6f52 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:00:45 +0200 Subject: [PATCH 046/316] remove timeout --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e5811a0..c531634 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -721,10 +721,10 @@ void TCPServer::awaitRobotIdle() { usleep(200'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; - if (timeout > 30) { + /*if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); break; - } + }*/ } } From 2f67211b01e068646231893c258a0159796f90e9 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:05:37 +0200 Subject: [PATCH 047/316] patch --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c531634..a1334c5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -768,7 +768,7 @@ std::optional TCPServer::getMostCenteredArucoTag(float borneMinX, floa bool found = false; ArucoTag mostCenteredTag = ArucoTag(); for (const auto & tag : arucoTags) { - if (tag.getNbFind() < 2) break; + if (tag.getNbFind() < 2) continue; if (!found) { if (tag.pos()[0] > borneMinX && tag.pos()[0] < borneMaxX && tag.pos()[1] > borneMinY && tag.pos()[1] < borneMaxY) { From 5b97c7f48d391d0f722671519f9746159720e8b1 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:13:58 +0200 Subject: [PATCH 048/316] drop position | speed --- TCPServer.cpp | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a1334c5..92038bf 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -429,8 +429,8 @@ void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { auto time = std::chrono::system_clock::now(); - // TODO check for the bbest timing to return to the spawn - if (time - gameStart > std::chrono::seconds(80)) { + // TODO check for the best timing to return to the spawn + if (time - gameStart > std::chrono::seconds(85)) { this->goEnd(); return; } @@ -675,8 +675,10 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double thetaPrime = std::atan2(centerPlantY, centerPlantX); + this->setSpeed(150); this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); awaitRobotIdle(); + this->setSpeed(200); double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX; double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY; @@ -852,15 +854,19 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(1000, 210); awaitRobotIdle(); + this->setSpeed(150); this->rotate(-PI/2); awaitRobotIdle(); + this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(1000, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->setSpeed(150); + this->rotate(PI/2); awaitRobotIdle(); + this->setSpeed(200); } else { return; } @@ -869,15 +875,19 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(2000, 210); awaitRobotIdle(); + this->setSpeed(150); this->rotate(-PI/2); awaitRobotIdle(); + this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(2000, 1790); awaitRobotIdle(); - this->rotate(PI / 2); + this->setSpeed(150); + this->rotate(PI/2); awaitRobotIdle(); + this->setSpeed(200); } else { return; } @@ -888,7 +898,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); this->broadcastMessage("strat;aruco;get aruco;1\n"); for (int i = 0; i < 4; i++) { - usleep(250'000); + usleep(500'000); this->broadcastMessage("strat;aruco;get aruco;1\n"); } usleep(100'000); @@ -931,7 +941,7 @@ void TCPServer::dropFlowers() { std::array purpleDropPosition{}; std::array whiteDropPosition{}; if (team == BLUE) { - purpleDropPosition = {300, 400}; + purpleDropPosition = {200, 300}; whiteDropPosition = {762, 300}; if (this->robotPose.pos.y > 1000) { checkpoints.emplace_back(std::array{500, 1700}); @@ -1025,7 +1035,6 @@ void TCPServer::dropFlowers() { void TCPServer::goAndTurnSolarPannel(StratPattern sp) { int previousSpeed = this->speed; this->setSpeed(150); - usleep(10'000); if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: @@ -1036,7 +1045,6 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(100'000); this->go(380, 1790); awaitRobotIdle(); @@ -1051,7 +1059,6 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(100'000); this->go(605, 1790); awaitRobotIdle(); @@ -1066,7 +1073,6 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(100'000); this->go(830, 1790); awaitRobotIdle(); @@ -1086,7 +1092,6 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(100'000); this->go(2620, 1790); awaitRobotIdle(); @@ -1101,7 +1106,6 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(100'000); this->go(2395, 1790); awaitRobotIdle(); @@ -1116,7 +1120,6 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(100'000); this->go(2170, 1790); awaitRobotIdle(); From e0fe2ac98c8bf6e0dc508a014614a7dedfe950cc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:15:12 +0200 Subject: [PATCH 049/316] speed when rotate --- TCPServer.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 92038bf..8cdcc3a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -966,13 +966,14 @@ void TCPServer::dropFlowers() { if (!pinceHavePurpleFlower.empty()) { this->go(purpleDropPosition); awaitRobotIdle(); + + this->setSpeed(150); + this->rotate(PI / 2); awaitRobotIdle(); this->leverBras(); - this->setSpeed(150); - for (auto & toDrop : pinceHavePurpleFlower) { this->openPince(toDrop); usleep(500'000); @@ -997,14 +998,14 @@ void TCPServer::dropFlowers() { this->go(whiteDropPosition); awaitRobotIdle(); + this->setSpeed(130); + this->rotate(PI / 2); awaitRobotIdle(); this->leverBras(); usleep(500'000); - this->setSpeed(130); - this->go(whiteDropPosition[0], 0); usleep(2'000'000); From 7ba9adb2bd7f8aae2fba904f47422fadb97411e1 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:21:45 +0200 Subject: [PATCH 050/316] speed and sleep --- TCPServer.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8cdcc3a..759ef6c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -854,7 +854,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(1000, 210); awaitRobotIdle(); - this->setSpeed(150); + this->setSpeed(170); this->rotate(-PI/2); awaitRobotIdle(); this->setSpeed(200); @@ -863,7 +863,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(1000, 1790); awaitRobotIdle(); - this->setSpeed(150); + this->setSpeed(170); this->rotate(PI/2); awaitRobotIdle(); this->setSpeed(200); @@ -875,7 +875,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(2000, 210); awaitRobotIdle(); - this->setSpeed(150); + this->setSpeed(170); this->rotate(-PI/2); awaitRobotIdle(); this->setSpeed(200); @@ -884,7 +884,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(2000, 1790); awaitRobotIdle(); - this->setSpeed(150); + this->setSpeed(170); this->rotate(PI/2); awaitRobotIdle(); this->setSpeed(200); @@ -1035,7 +1035,7 @@ void TCPServer::dropFlowers() { void TCPServer::goAndTurnSolarPannel(StratPattern sp) { int previousSpeed = this->speed; - this->setSpeed(150); + this->setSpeed(170); if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: @@ -1046,6 +1046,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); + usleep(50'000); this->go(380, 1790); awaitRobotIdle(); @@ -1060,6 +1061,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); + usleep(50'000); this->go(605, 1790); awaitRobotIdle(); @@ -1074,6 +1076,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); + usleep(50'000); this->go(830, 1790); awaitRobotIdle(); @@ -1093,6 +1096,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); + usleep(50'000); this->go(2620, 1790); awaitRobotIdle(); @@ -1107,6 +1111,8 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); + usleep(50'000); + this->go(2395, 1790); awaitRobotIdle(); @@ -1121,6 +1127,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); + usleep(50'000); this->go(2170, 1790); awaitRobotIdle(); From af3254f086b0b7e329f3ff730404d999433ef79c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:30:24 +0200 Subject: [PATCH 051/316] aruco things --- TCPServer.cpp | 42 ++++++++++++++++++++++++++++-------------- 1 file changed, 28 insertions(+), 14 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 759ef6c..c61e573 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -810,13 +810,20 @@ void TCPServer::handleEmergency(int distance, double angle) { void TCPServer::startTestAruco(int pince) { this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 5; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - std::optional tag = getBiggestArucoTag(300, 1500, -600, 600); + std::optional tag = std::nullopt; + + int timeout = 0; + while (!tag.has_value()) { + this->broadcastMessage("strat;aruco;get aruco;1\n"); + usleep(500'000); + tag = getMostCenteredArucoTag(100, 800, -400, 400); + + timeout++; + if (timeout > 10) { + break; + } + } if (tag.has_value()) { goToAruco(tag.value(), pince); @@ -895,15 +902,22 @@ void TCPServer::findAndGoFlower(StratPattern sp) { return; } - this->arucoTags.clear(); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - for (int i = 0; i < 4; i++) { - usleep(500'000); - this->broadcastMessage("strat;aruco;get aruco;1\n"); - } - usleep(100'000); - std::optional tag = getMostCenteredArucoTag(100, 800, -200, 200); + this->arucoTags.clear(); + + std::optional tag = std::nullopt; + + int timeout = 0; + while (!tag.has_value()) { + this->broadcastMessage("strat;aruco;get aruco;1\n"); + usleep(500'000); + tag = getMostCenteredArucoTag(100, 800, -400, 400); + + timeout++; + if (timeout > 10) { + break; + } + } if (tag.has_value()) { if (pinceState[1] == NONE) { From 62fc61e625ce60761513c745657c3e2c880f53e3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:40:13 +0200 Subject: [PATCH 052/316] aruco things --- TCPServer.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c61e573..e0a4ba5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -904,17 +904,22 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); - std::optional tag = std::nullopt; + for (int i = 0; i < 3; i++) { + this->broadcastMessage("strat;aruco;get aruco;1\n"); + usleep(220'000); + } + tag = getMostCenteredArucoTag(100, 800, -400, 400); + int timeout = 0; while (!tag.has_value()) { this->broadcastMessage("strat;aruco;get aruco;1\n"); - usleep(500'000); + usleep(220'000); tag = getMostCenteredArucoTag(100, 800, -400, 400); timeout++; - if (timeout > 10) { + if (timeout > 5) { break; } } From 7128ce7799be83dc6e78332910d979111832a448 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 11:49:30 +0200 Subject: [PATCH 053/316] speed --- TCPServer.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e0a4ba5..a5b444a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -861,19 +861,15 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(1000, 210); awaitRobotIdle(); - this->setSpeed(170); this->rotate(-PI/2); awaitRobotIdle(); - this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(1000, 1790); awaitRobotIdle(); - this->setSpeed(170); this->rotate(PI/2); awaitRobotIdle(); - this->setSpeed(200); } else { return; } @@ -882,19 +878,15 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(2000, 210); awaitRobotIdle(); - this->setSpeed(170); this->rotate(-PI/2); awaitRobotIdle(); - this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(2000, 1790); awaitRobotIdle(); - this->setSpeed(170); this->rotate(PI/2); awaitRobotIdle(); - this->setSpeed(200); } else { return; } @@ -906,7 +898,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); std::optional tag = std::nullopt; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 4; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(220'000); } From 25eb3b2bbb05cb598e9082000ef1217b8bb39166 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 12:02:11 +0200 Subject: [PATCH 054/316] speed --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a5b444a..455c443 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -675,10 +675,8 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double thetaPrime = std::atan2(centerPlantY, centerPlantX); - this->setSpeed(150); this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); awaitRobotIdle(); - this->setSpeed(200); double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX; double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY; From b21a46cd233155dd4e2ac2df45af5bae6a68e16f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 12:16:20 +0200 Subject: [PATCH 055/316] speed --- TCPServer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 455c443..60f7a24 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -859,15 +859,19 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(1000, 210); awaitRobotIdle(); + this->setSpeed(190); this->rotate(-PI/2); awaitRobotIdle(); + this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(1000, 1790); awaitRobotIdle(); + this->setSpeed(190); this->rotate(PI/2); awaitRobotIdle(); + this->setSpeed(190); } else { return; } @@ -876,15 +880,19 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(2000, 210); awaitRobotIdle(); + this->setSpeed(190); this->rotate(-PI/2); awaitRobotIdle(); + this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(2000, 1790); awaitRobotIdle(); + this->setSpeed(190); this->rotate(PI/2); awaitRobotIdle(); + this->setSpeed(200); } else { return; } From 5bc1f00cdb9da20be7fa333692333e462853ce30 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 12:27:22 +0200 Subject: [PATCH 056/316] speed | checkpoint --- TCPServer.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 60f7a24..30d898b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -718,7 +718,7 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAEndlessLoop usleep(100'000); while (isRobotIdle < 2) { - usleep(200'000); + usleep(100'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; /*if (timeout > 30) { @@ -846,6 +846,7 @@ void TCPServer::goEnd() { this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); awaitRobotIdle(); + this->setSpeed(180); this->rotate(this->endRobotPose.theta); awaitRobotIdle(); @@ -859,7 +860,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(1000, 210); awaitRobotIdle(); - this->setSpeed(190); + this->setSpeed(180); this->rotate(-PI/2); awaitRobotIdle(); this->setSpeed(200); @@ -868,7 +869,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(1000, 1790); awaitRobotIdle(); - this->setSpeed(190); + this->setSpeed(180); this->rotate(PI/2); awaitRobotIdle(); this->setSpeed(190); @@ -880,7 +881,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(2000, 210); awaitRobotIdle(); - this->setSpeed(190); + this->setSpeed(180); this->rotate(-PI/2); awaitRobotIdle(); this->setSpeed(200); @@ -889,7 +890,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->go(2000, 1790); awaitRobotIdle(); - this->setSpeed(190); + this->setSpeed(180); this->rotate(PI/2); awaitRobotIdle(); this->setSpeed(200); @@ -961,14 +962,14 @@ void TCPServer::dropFlowers() { purpleDropPosition = {200, 300}; whiteDropPosition = {762, 300}; if (this->robotPose.pos.y > 1000) { - checkpoints.emplace_back(std::array{500, 1700}); + checkpoints.emplace_back(std::array{500, 1500}); checkpoints.emplace_back(std::array{500, 500}); } } else if (team == YELLOW) { purpleDropPosition = {2700, 400}; whiteDropPosition = {2237, 400}; if (this->robotPose.pos.y > 1000) { - checkpoints.emplace_back(std::array{2500, 1700}); + checkpoints.emplace_back(std::array{2500, 1500}); checkpoints.emplace_back(std::array{2500, 500}); } } From 7d28ba563f3b0582b46ed7b987f34b548728657e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 12:30:56 +0200 Subject: [PATCH 057/316] speed | checkpoint --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 30d898b..0c24b13 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -994,7 +994,7 @@ void TCPServer::dropFlowers() { for (auto & toDrop : pinceHavePurpleFlower) { this->openPince(toDrop); - usleep(500'000); + usleep(800'000); this->go(this->robotPose.pos.x, this->robotPose.pos.y + 50); awaitRobotIdle(); @@ -1030,7 +1030,7 @@ void TCPServer::dropFlowers() { for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { this->openPince(i); - usleep(500'000); + usleep(800'000); pinceState[i] = NONE; this->closePince(i); From 8b69b3887746d1474aff0a0456034329d87f067f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 16:14:31 +0200 Subject: [PATCH 058/316] aruco --- TCPServer.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0c24b13..62cdafb 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -716,15 +716,14 @@ void TCPServer::awaitRobotIdle() { int timeout = 0; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop - usleep(100'000); while (isRobotIdle < 2) { - usleep(100'000); + usleep(50'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; - /*if (timeout > 30) { + if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); break; - }*/ + } } } @@ -808,17 +807,22 @@ void TCPServer::handleEmergency(int distance, double angle) { void TCPServer::startTestAruco(int pince) { this->arucoTags.clear(); - std::optional tag = std::nullopt; + for (int i = 0; i < 5; i++) { + this->broadcastMessage("strat;aruco;get aruco;1\n"); + usleep(220'000); + } + tag = getMostCenteredArucoTag(100, 800, -400, 400); + int timeout = 0; while (!tag.has_value()) { this->broadcastMessage("strat;aruco;get aruco;1\n"); - usleep(500'000); + usleep(220'000); tag = getMostCenteredArucoTag(100, 800, -400, 400); timeout++; - if (timeout > 10) { + if (timeout > 5) { break; } } @@ -905,7 +909,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); std::optional tag = std::nullopt; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 5; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(220'000); } From e663fbf33ceb9fb8ebaa686b149dea8d1588cc50 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 16:51:44 +0200 Subject: [PATCH 059/316] test --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 62cdafb..c33cdcf 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -670,7 +670,9 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double yPrime = arucoTag.pos()[1]; double roll = arucoTag.rot()[1]; - auto centerPlantX = (20 * std::cos(roll)) + xPrime; + double test = (xPrime - 500) * 0.1; + + auto centerPlantX = (20 * std::cos(roll)) + xPrime + test; auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); From ea69dc217cd0f2228c34efd1fd2720345b18677c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 16:57:57 +0200 Subject: [PATCH 060/316] test --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c33cdcf..263e164 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -670,7 +670,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double yPrime = arucoTag.pos()[1]; double roll = arucoTag.rot()[1]; - double test = (xPrime - 500) * 0.1; + double test = (xPrime - 400) * 0.1; auto centerPlantX = (20 * std::cos(roll)) + xPrime + test; auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; From c52c8e159c675f7bb327a0e0042220b11c03dc72 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 16:58:14 +0200 Subject: [PATCH 061/316] sleep --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 263e164..070f968 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -719,7 +719,7 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop while (isRobotIdle < 2) { - usleep(50'000); + usleep(100'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; if (timeout > 30) { From 071cabe6c3932e2c138d4c404cfd54f92002793b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 30 Apr 2024 17:10:12 +0200 Subject: [PATCH 062/316] remove timeout --- TCPServer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 070f968..0e57112 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -722,10 +722,10 @@ void TCPServer::awaitRobotIdle() { usleep(100'000); this->broadcastMessage("strat;arduino;get state;1\n"); timeout++; - if (timeout > 30) { + /*if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); break; - } + }*/ } } @@ -911,6 +911,7 @@ void TCPServer::findAndGoFlower(StratPattern sp) { this->arucoTags.clear(); std::optional tag = std::nullopt; + usleep(500'000); for (int i = 0; i < 5; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(220'000); From 8c1845e930e77d68667c59aa4c494d440d18a368 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=83ackimixs?= <Ãackimixs@gmail.com> Date: Wed, 1 May 2024 13:16:12 +0200 Subject: [PATCH 063/316] patch solar panel --- TCPServer.cpp | 57 +++++++++++++++------------------------------------ TCPServer.h | 2 +- 2 files changed, 17 insertions(+), 42 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0e57112..3001f9b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -437,13 +437,13 @@ void TCPServer::startGame() { switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_1); + goAndTurnSolarPanel(TURN_SOLAR_PANNEL_1); break; case TURN_SOLAR_PANNEL_2: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_2); + goAndTurnSolarPanel(TURN_SOLAR_PANNEL_2); break; case TURN_SOLAR_PANNEL_3: - goAndTurnSolarPannel(TURN_SOLAR_PANNEL_3); + goAndTurnSolarPanel(TURN_SOLAR_PANNEL_3); break; case TAKE_FLOWER_TOP: findAndGoFlower(TAKE_FLOWER_TOP); @@ -1058,7 +1058,7 @@ void TCPServer::dropFlowers() { } } -void TCPServer::goAndTurnSolarPannel(StratPattern sp) { +void TCPServer::goAndTurnSolarPanel(StratPattern sp) { int previousSpeed = this->speed; this->setSpeed(170); if (team == BLUE) { @@ -1071,11 +1071,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(50'000); - - this->go(380, 1790); - awaitRobotIdle(); - + usleep(200'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: @@ -1086,11 +1082,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(50'000); - - this->go(605, 1790); - awaitRobotIdle(); - + usleep(200'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: @@ -1101,11 +1093,7 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(50'000); - - this->go(830, 1790); - awaitRobotIdle(); - + usleep(200'000); this->uncheckPanneau(7); break; default: @@ -1120,13 +1108,9 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(6); - usleep(50'000); - - this->go(2620, 1790); - awaitRobotIdle(); - - this->uncheckPanneau(6); + this->checkPanneau(7); + usleep(200'000); + this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: this->go(2525, 1790); @@ -1135,14 +1119,9 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(6); - usleep(50'000); - - - this->go(2395, 1790); - awaitRobotIdle(); - - this->uncheckPanneau(6); + this->checkPanneau(7); + usleep(200'000); + this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: this->go(2300, 1790); @@ -1151,13 +1130,9 @@ void TCPServer::goAndTurnSolarPannel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(6); - usleep(50'000); - - this->go(2170, 1790); - awaitRobotIdle(); - - this->uncheckPanneau(6); + this->checkPanneau(7); + usleep(200'000); + this->uncheckPanneau(7); break; default: break; diff --git a/TCPServer.h b/TCPServer.h index ccde6d4..0314c4b 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -184,7 +184,7 @@ public: /* * Start Strategy function */ - void goAndTurnSolarPannel(StratPattern sp); + void goAndTurnSolarPanel(StratPattern sp); void findAndGoFlower(StratPattern sp); From bca6b5314adb9bc7830d42431da00a10caf073c8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 1 May 2024 22:42:06 +0200 Subject: [PATCH 064/316] checkpoint and drop flower --- TCPServer.cpp | 87 ++++++++++++++++++++++++++++++++++++--------------- TCPServer.h | 9 ++++-- 2 files changed, 68 insertions(+), 28 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3001f9b..747c7a0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -460,6 +460,12 @@ void TCPServer::startGame() { case GET_LIDAR_POS: getLidarPos(); break; + case CHECKPOINT_BOTTOM_TO_TOP: + checkpoint(CHECKPOINT_BOTTOM_TO_TOP); + break; + case CHECKPOINT_TOP_TO_BOTTOM: + checkpoint(CHECKPOINT_TOP_TO_BOTTOM); + break; } whereAmI++; } @@ -692,6 +698,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { usleep(500'000); this->setSpeed(previousSpeed); pinceState[pince] = TCPUtils::startWith(arucoTag.name(), "Purple_flower") ? PURPLE_FLOWER : WHITE_FLOWER; + this->transportBras(); } void TCPServer::askArduinoPos() { @@ -859,7 +866,7 @@ void TCPServer::goEnd() { this->broadcastMessage("strat;all;end;1"); } -void TCPServer::findAndGoFlower(StratPattern sp) { +void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { @@ -945,8 +952,6 @@ void TCPServer::dropFlowers() { std::vector pinceHavePurpleFlower; std::vector pinceHaveWhiteFlower; - std::vector> checkpoints; - pinceHavePurpleFlower.reserve(3); pinceHaveWhiteFlower.reserve(3); @@ -966,29 +971,17 @@ void TCPServer::dropFlowers() { std::array purpleDropPosition{}; std::array whiteDropPosition{}; if (team == BLUE) { - purpleDropPosition = {200, 300}; - whiteDropPosition = {762, 300}; - if (this->robotPose.pos.y > 1000) { - checkpoints.emplace_back(std::array{500, 1500}); - checkpoints.emplace_back(std::array{500, 500}); - } + purpleDropPosition = {300, 225}; + whiteDropPosition = firstTimeDropWhiteFlower ? std::array{755, 300} : std::array{765, 300}; } else if (team == YELLOW) { - purpleDropPosition = {2700, 400}; - whiteDropPosition = {2237, 400}; - if (this->robotPose.pos.y > 1000) { - checkpoints.emplace_back(std::array{2500, 1500}); - checkpoints.emplace_back(std::array{2500, 500}); - } - } - - for (const auto& checkpoint : checkpoints) { - this->go(checkpoint); - awaitRobotIdle(); + purpleDropPosition = {2700, 225}; + whiteDropPosition = firstTimeDropWhiteFlower ? std::array{2227, 300} : std::array{2237, 300}; } this->setSpeed(200); if (!pinceHavePurpleFlower.empty()) { + // TODO change that this->go(purpleDropPosition); awaitRobotIdle(); @@ -997,24 +990,25 @@ void TCPServer::dropFlowers() { this->rotate(PI / 2); awaitRobotIdle(); - this->leverBras(); + this->baisserBras(); for (auto & toDrop : pinceHavePurpleFlower) { this->openPince(toDrop); - usleep(800'000); + usleep(200'000); - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 50); + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 100); awaitRobotIdle(); pinceState[toDrop] = NONE; this->closePince(toDrop); - usleep(100'000); + usleep(200'000); + + this->go(this->robotPose.pos.x, this->robotPose.pos.y - 100); + awaitRobotIdle(); } this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); awaitRobotIdle(); - - this->baisserBras(); } this->setSpeed(200); @@ -1055,6 +1049,8 @@ void TCPServer::dropFlowers() { awaitRobotIdle(); this->baisserBras(); + + firstTimeDropWhiteFlower++; } } @@ -1142,6 +1138,45 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { this->setSpeed(previousSpeed); } +void TCPServer::checkpoint(StratPattern sp) { + if (team == BLUE) { + switch (sp) { + case CHECKPOINT_BOTTOM_TO_TOP: + this->go(500, 1500); + this->awaitRobotIdle(); + this->go(500, 500); + this->awaitRobotIdle(); + break; + case CHECKPOINT_TOP_TO_BOTTOM: + this->go(500, 500); + this->awaitRobotIdle(); + this->go(500, 1500); + this->awaitRobotIdle(); + break; + default: + break; + } + } else if (team == YELLOW) { + switch (sp) { + case CHECKPOINT_BOTTOM_TO_TOP: + this->go(1500, 1500); + this->awaitRobotIdle(); + this->go(1500, 500); + this->awaitRobotIdle(); + break; + case CHECKPOINT_TOP_TO_BOTTOM: + this->go(1500, 500); + this->awaitRobotIdle(); + this->go(1500, 1500); + this->awaitRobotIdle(); + break; + default: + break; + } + } +} + + void TCPServer::getLidarPos() { this->askLidarPosition(); diff --git a/TCPServer.h b/TCPServer.h index 0314c4b..216cec7 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -43,6 +43,8 @@ enum StratPattern { DROP_FLOWER, GO_END, GET_LIDAR_POS, + CHECKPOINT_BOTTOM_TO_TOP, + CHECKPOINT_TOP_TO_BOTTOM }; class TCPServer; // Forward declaration @@ -102,11 +104,10 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, - // GET_LIDAR_POS, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, - // GET_LIDAR_POS, + CHECKPOINT_BOTTOM_TO_TOP, DROP_FLOWER, // GET_LIDAR_POS, TAKE_FLOWER_TOP, @@ -134,6 +135,8 @@ private: int lidarSocket = -1; + int firstTimeDropWhiteFlower = 0; + public: explicit TCPServer(int port); @@ -193,6 +196,8 @@ public: void dropFlowers(); void getLidarPos(); + + void checkpoint(StratPattern sp); /* * End Strategy function */ From 9531a2f4aa410d06b7a87f0141f20b861066f6df Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 09:47:00 +0200 Subject: [PATCH 065/316] purple flower --- TCPServer.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 747c7a0..e5d32d0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -972,16 +972,15 @@ void TCPServer::dropFlowers() { std::array whiteDropPosition{}; if (team == BLUE) { purpleDropPosition = {300, 225}; - whiteDropPosition = firstTimeDropWhiteFlower ? std::array{755, 300} : std::array{765, 300}; + whiteDropPosition = firstTimeDropWhiteFlower ? std::array{755, 400} : std::array{765, 400}; } else if (team == YELLOW) { purpleDropPosition = {2700, 225}; - whiteDropPosition = firstTimeDropWhiteFlower ? std::array{2227, 300} : std::array{2237, 300}; + whiteDropPosition = firstTimeDropWhiteFlower ? std::array{2227, 400} : std::array{2237, 400}; } this->setSpeed(200); if (!pinceHavePurpleFlower.empty()) { - // TODO change that this->go(purpleDropPosition); awaitRobotIdle(); @@ -996,19 +995,16 @@ void TCPServer::dropFlowers() { this->openPince(toDrop); usleep(200'000); - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 100); + this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150); + awaitRobotIdle(); + + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); awaitRobotIdle(); pinceState[toDrop] = NONE; this->closePince(toDrop); usleep(200'000); - - this->go(this->robotPose.pos.x, this->robotPose.pos.y - 100); - awaitRobotIdle(); } - - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); - awaitRobotIdle(); } this->setSpeed(200); From 5f514968f719066e06b732ef18e5d024f1f5c02c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 11:41:53 +0200 Subject: [PATCH 066/316] timeout | TODO | pince --- TCPServer.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e5d32d0..7ca0e63 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -216,6 +216,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) spawnPoint[0] = 1200; spawnPoint[1] = 1800; spawnPoint[2] = 1.57; + finishPoint[0] = 1200; finishPoint[1] = 1800; finishPoint[2] = 1.57; @@ -429,8 +430,7 @@ void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { auto time = std::chrono::system_clock::now(); - // TODO check for the best timing to return to the spawn - if (time - gameStart > std::chrono::seconds(85)) { + if (time - gameStart > std::chrono::seconds(82)) { this->goEnd(); return; } @@ -737,7 +737,7 @@ void TCPServer::awaitRobotIdle() { } void TCPServer::handleArucoTag(ArucoTag &tag) { - if (!TCPUtils::endWith(tag.name(), "flower")) { + if (!TCPUtils::contains(tag.name(), "flower")) { return; } @@ -932,7 +932,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { tag = getMostCenteredArucoTag(100, 800, -400, 400); timeout++; - if (timeout > 5) { + if (timeout > 3) { break; } } @@ -1044,7 +1044,11 @@ void TCPServer::dropFlowers() { this->go(whiteDropPosition); awaitRobotIdle(); - this->baisserBras(); + for (int i = 0; i < 3; i++) { + this->closePince(i); + } + + this->transportBras(); firstTimeDropWhiteFlower++; } From 6199bbe58c1b72e1b46d87323bd9d6591e6a5334 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 11:47:33 +0200 Subject: [PATCH 067/316] flower checkpoint --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7ca0e63..044d597 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -879,7 +879,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1790); + this->go(1000, 1700); awaitRobotIdle(); this->setSpeed(180); @@ -900,7 +900,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1790); + this->go(2000, 1700); awaitRobotIdle(); this->setSpeed(180); From 83a3ee26788c09d6d545a27b0ebecbb0f6c5c47a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 11:55:07 +0200 Subject: [PATCH 068/316] arduino socket --- TCPServer.cpp | 9 ++++----- TCPServer.h | 1 + 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 044d597..87b58e2 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -702,20 +702,19 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { } void TCPServer::askArduinoPos() { - ClientTCP arduino; for (const auto & client : clients) { if (client.name == "arduino") { - arduino = client; + this->arduinoSocket = client.socket; break; } } - if (arduino.socket == -1) { + if (this->arduinoSocket == -1) { return; } while (!this->_shouldStop) { - this->sendToClient("strat;arduino;get pos;1\n", arduino.socket); + this->sendToClient("strat;arduino;get pos;1\n", this->arduinoSocket); usleep(200'000); } } @@ -727,7 +726,7 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAEndlessLoop while (isRobotIdle < 2) { usleep(100'000); - this->broadcastMessage("strat;arduino;get state;1\n"); + this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; /*if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); diff --git a/TCPServer.h b/TCPServer.h index 216cec7..2d06186 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -134,6 +134,7 @@ private: std::thread gameThread; int lidarSocket = -1; + int arduinoSocket = -1; int firstTimeDropWhiteFlower = 0; From 93e9cf865d246654b1dc60ab0cab3c88aadbd7a0 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 12:04:17 +0200 Subject: [PATCH 069/316] purple flower --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 87b58e2..712306a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -970,11 +970,11 @@ void TCPServer::dropFlowers() { std::array purpleDropPosition{}; std::array whiteDropPosition{}; if (team == BLUE) { - purpleDropPosition = {300, 225}; - whiteDropPosition = firstTimeDropWhiteFlower ? std::array{755, 400} : std::array{765, 400}; + purpleDropPosition = {300, 400}; + whiteDropPosition = firstTimeDropWhiteFlower ? std::array{755, 300} : std::array{765, 300}; } else if (team == YELLOW) { - purpleDropPosition = {2700, 225}; - whiteDropPosition = firstTimeDropWhiteFlower ? std::array{2227, 400} : std::array{2237, 400}; + purpleDropPosition = {2700, 400}; + whiteDropPosition = firstTimeDropWhiteFlower ? std::array{2227, 300} : std::array{2237, 300}; } this->setSpeed(200); From e85fe397784f9f92e3744407113afa1c31cdffc4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 12:10:26 +0200 Subject: [PATCH 070/316] checkpoint plants --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 712306a..21d7d47 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -869,7 +869,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { - this->go(1000, 210); + this->go(1000, 250); awaitRobotIdle(); this->setSpeed(180); @@ -878,7 +878,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1700); + this->go(1000, 1750); awaitRobotIdle(); this->setSpeed(180); @@ -890,7 +890,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { - this->go(2000, 210); + this->go(2000, 250); awaitRobotIdle(); this->setSpeed(180); @@ -899,7 +899,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1700); + this->go(2000, 1750); awaitRobotIdle(); this->setSpeed(180); From c65de383ffd7e95391c5db2730831d36725bc009 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 14:19:20 +0200 Subject: [PATCH 071/316] bottom flower blue team --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 21d7d47..afd7557 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -878,11 +878,11 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1750); + this->go(1300, 350); awaitRobotIdle(); this->setSpeed(180); - this->rotate(PI/2); + this->rotate(0); awaitRobotIdle(); this->setSpeed(190); } else { From 35213c73d96fcb3ec9f88fde8ba029ed2a875f60 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 14:23:33 +0200 Subject: [PATCH 072/316] bottom flower blue team --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index afd7557..0dd9e18 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -878,7 +878,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1300, 350); + this->go(350, 1300); awaitRobotIdle(); this->setSpeed(180); From beb561b563475ddfe1618f600a1bf555e8ef6891 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 15:02:25 +0200 Subject: [PATCH 073/316] bottom flower blue team --- TCPServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0dd9e18..07e7ca1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -878,11 +878,11 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(350, 1300); + this->go(1000, 1790); awaitRobotIdle(); this->setSpeed(180); - this->rotate(0); + this->rotate(PI/2); awaitRobotIdle(); this->setSpeed(190); } else { @@ -899,7 +899,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1750); + this->go(2000, 1790); awaitRobotIdle(); this->setSpeed(180); From f85714f0bb3aadd966ac2371f911dee99ba9a94a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 15:24:07 +0200 Subject: [PATCH 074/316] sleep --- TCPServer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 07e7ca1..a4ceb1a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1026,7 +1026,7 @@ void TCPServer::dropFlowers() { for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { this->openPince(i); - usleep(800'000); + usleep(1'000'000); pinceState[i] = NONE; this->closePince(i); @@ -1037,6 +1037,7 @@ void TCPServer::dropFlowers() { for (int i = 0; i < 3; i++) { this->middlePince(i); } + usleep(1'000'000); this->setSpeed(200); From d637ae8c4f67132d9cb10fee400aeebb11b54a08 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 15:26:37 +0200 Subject: [PATCH 075/316] borne --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a4ceb1a..e1e490c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -922,13 +922,13 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(220'000); } - tag = getMostCenteredArucoTag(100, 800, -400, 400); + tag = getMostCenteredArucoTag(100, 800, -300, 300); int timeout = 0; while (!tag.has_value()) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(220'000); - tag = getMostCenteredArucoTag(100, 800, -400, 400); + tag = getMostCenteredArucoTag(100, 800, -300, 300); timeout++; if (timeout > 3) { From 258bad46f9d77bc682a1ab73f9dd4ddede1048a3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 15:44:59 +0200 Subject: [PATCH 076/316] sleep --- TCPServer.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e1e490c..8931890 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1067,7 +1067,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(200'000); + usleep(500'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: @@ -1078,7 +1078,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(200'000); + usleep(500'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: @@ -1089,7 +1089,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(200'000); + usleep(500'000); this->uncheckPanneau(7); break; default: @@ -1104,9 +1104,9 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(7); - usleep(200'000); - this->uncheckPanneau(7); + this->checkPanneau(6); + usleep(500'000); + this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: this->go(2525, 1790); @@ -1115,9 +1115,9 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(7); - usleep(200'000); - this->uncheckPanneau(7); + this->checkPanneau(6); + usleep(500'000); + this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: this->go(2300, 1790); @@ -1126,9 +1126,9 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { this->rotate(PI); awaitRobotIdle(); - this->checkPanneau(7); - usleep(200'000); - this->uncheckPanneau(7); + this->checkPanneau(6); + usleep(500'000); + this->uncheckPanneau(6); break; default: break; From d5f3ac92b49448108c870b8acc000a8d7ad4edea Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 15:52:37 +0200 Subject: [PATCH 077/316] sleep --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8931890..86bb255 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1067,7 +1067,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(500'000); + usleep(300'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: @@ -1078,7 +1078,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(500'000); + usleep(300'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: @@ -1089,7 +1089,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(500'000); + usleep(300'000); this->uncheckPanneau(7); break; default: @@ -1105,7 +1105,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(500'000); + usleep(300'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: @@ -1116,7 +1116,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(500'000); + usleep(300'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: @@ -1127,7 +1127,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(500'000); + usleep(300'000); this->uncheckPanneau(6); break; default: From 5a148704a0fdf1e03c07d748d42390a40eea22f5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 20:01:14 +0200 Subject: [PATCH 078/316] distance --- TCPServer.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 86bb255..7a5f11b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -676,9 +676,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double yPrime = arucoTag.pos()[1]; double roll = arucoTag.rot()[1]; - double test = (xPrime - 400) * 0.1; - - auto centerPlantX = (20 * std::cos(roll)) + xPrime + test; + auto centerPlantX = (20 * std::cos(roll)) + xPrime; auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); From 89a5d17521bd896104ec48e1c829f35a232e69e5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 20:25:57 +0200 Subject: [PATCH 079/316] change drop system --- TCPServer.cpp | 65 ++++++++++++++++++++++++++++++++++++--------------- TCPServer.h | 18 +++++++++----- 2 files changed, 58 insertions(+), 25 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7a5f11b..b5b87a8 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -454,8 +454,14 @@ void TCPServer::startGame() { case GO_END: goEnd(); break; - case DROP_FLOWER: - dropFlowers(); + case DROP_PURPLE_FLOWER: + dropPurpleFlowers(); + break; + case DROP_WHITE_FLOWER_J1: + dropWhiteFlowers(DROP_WHITE_FLOWER_J1); + break; + case DROP_WHITE_FLOWER_J2: + dropWhiteFlowers(DROP_WHITE_FLOWER_J2); break; case GET_LIDAR_POS: getLidarPos(); @@ -680,14 +686,16 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); + int previousSpeed = this->speed; + this->setSpeed(180); this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); awaitRobotIdle(); + this->setSpeed(200); double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX; double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY; - int previousSpeed = this->speed; this->transit(robotPosForPotX, robotPosForPotY, 130); awaitRobotIdle(); @@ -945,12 +953,10 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { } } -void TCPServer::dropFlowers() { +void TCPServer::dropPurpleFlowers() { std::vector pinceHavePurpleFlower; - std::vector pinceHaveWhiteFlower; pinceHavePurpleFlower.reserve(3); - pinceHaveWhiteFlower.reserve(3); for (int i = 0; i < 3; i++) { switch (pinceState[i]) { @@ -958,21 +964,16 @@ void TCPServer::dropFlowers() { pinceHavePurpleFlower.push_back(i); break; case WHITE_FLOWER: - pinceHaveWhiteFlower.push_back(i); - break; case NONE: break; } } std::array purpleDropPosition{}; - std::array whiteDropPosition{}; if (team == BLUE) { purpleDropPosition = {300, 400}; - whiteDropPosition = firstTimeDropWhiteFlower ? std::array{755, 300} : std::array{765, 300}; } else if (team == YELLOW) { purpleDropPosition = {2700, 400}; - whiteDropPosition = firstTimeDropWhiteFlower ? std::array{2227, 300} : std::array{2237, 300}; } this->setSpeed(200); @@ -1004,6 +1005,34 @@ void TCPServer::dropFlowers() { } } + this->transportBras(); + + this->setSpeed(200); +} + +void TCPServer::dropWhiteFlowers(StratPattern sp) { + std::vector pinceHaveWhiteFlower; + + pinceHaveWhiteFlower.reserve(3); + + for (int i = 0; i < 3; i++) { + switch (pinceState[i]) { + case WHITE_FLOWER: + pinceHaveWhiteFlower.push_back(i); + break; + case PURPLE_FLOWER: + case NONE: + break; + } + } + + std::array whiteDropPosition{}; + if (team == BLUE) { + whiteDropPosition = std::array{775, 300}; + } else if (team == YELLOW) { + whiteDropPosition = std::array{2224, 300}; + } + this->setSpeed(200); if (!pinceHaveWhiteFlower.empty()) { @@ -1047,8 +1076,6 @@ void TCPServer::dropFlowers() { } this->transportBras(); - - firstTimeDropWhiteFlower++; } } @@ -1065,7 +1092,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(300'000); + usleep(150'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: @@ -1076,7 +1103,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(300'000); + usleep(150'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: @@ -1087,7 +1114,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(300'000); + usleep(150'000); this->uncheckPanneau(7); break; default: @@ -1103,7 +1130,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(300'000); + usleep(150'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: @@ -1114,7 +1141,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(300'000); + usleep(150'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: @@ -1125,7 +1152,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(300'000); + usleep(150'000); this->uncheckPanneau(6); break; default: diff --git a/TCPServer.h b/TCPServer.h index 2d06186..86963ad 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -40,7 +40,9 @@ enum StratPattern { TURN_SOLAR_PANNEL_3, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_TOP, - DROP_FLOWER, + DROP_PURPLE_FLOWER, + DROP_WHITE_FLOWER_J1, + DROP_WHITE_FLOWER_J2, GO_END, GET_LIDAR_POS, CHECKPOINT_BOTTOM_TO_TOP, @@ -108,13 +110,17 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, CHECKPOINT_BOTTOM_TO_TOP, - DROP_FLOWER, + // DROP_FLOWER, + DROP_PURPLE_FLOWER, + DROP_WHITE_FLOWER_J1, // GET_LIDAR_POS, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, // GET_LIDAR_POS, - DROP_FLOWER, + // DROP_FLOWER, + DROP_PURPLE_FLOWER, + DROP_WHITE_FLOWER_J2, // GET_LIDAR_POS, /* TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, @@ -136,8 +142,6 @@ private: int lidarSocket = -1; int arduinoSocket = -1; - int firstTimeDropWhiteFlower = 0; - public: explicit TCPServer(int port); @@ -194,7 +198,9 @@ public: void goEnd(); - void dropFlowers(); + void dropPurpleFlowers(); + + void dropWhiteFlowers(StratPattern sp); void getLidarPos(); From 91389baf4669f70df8d8383214e6ede6fa7d65cf Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 20:52:19 +0200 Subject: [PATCH 080/316] drop white flower --- TCPServer.cpp | 95 +++++++++++++++++++++++++++++++-------------------- 1 file changed, 58 insertions(+), 37 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b5b87a8..cd66ee1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -971,9 +971,9 @@ void TCPServer::dropPurpleFlowers() { std::array purpleDropPosition{}; if (team == BLUE) { - purpleDropPosition = {300, 400}; + purpleDropPosition = {225, 400}; } else if (team == YELLOW) { - purpleDropPosition = {2700, 400}; + purpleDropPosition = {2775, 400}; } this->setSpeed(200); @@ -1026,57 +1026,78 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { } } + if (pinceHaveWhiteFlower.empty()) { + return; + } + + std::array whiteDropSetup{}; std::array whiteDropPosition{}; + double angle = PI / 2; if (team == BLUE) { - whiteDropPosition = std::array{775, 300}; + if (sp == DROP_WHITE_FLOWER_J1) { + whiteDropSetup = std::array{775, 300}; + whiteDropPosition = std::array{775, 0}; + angle = PI / 2; + } else if (sp == DROP_WHITE_FLOWER_J2) { + whiteDropSetup = std::array{200, 400}; + whiteDropPosition = std::array{0, 400}; + angle = -PI; + } } else if (team == YELLOW) { - whiteDropPosition = std::array{2224, 300}; + if (sp == DROP_WHITE_FLOWER_J1) { + whiteDropSetup = std::array{2224, 300}; + whiteDropPosition = std::array{2224, 0}; + angle = PI / 2; + } else if (sp == DROP_WHITE_FLOWER_J2) { + whiteDropSetup = std::array{2800, 400}; + whiteDropPosition = std::array{3000, 400}; + angle = 0; + + } } this->setSpeed(200); - if (!pinceHaveWhiteFlower.empty()) { - this->go(whiteDropPosition); - awaitRobotIdle(); + this->go(whiteDropSetup); + awaitRobotIdle(); - this->setSpeed(130); + this->setSpeed(130); - this->rotate(PI / 2); - awaitRobotIdle(); + this->rotate(angle); + awaitRobotIdle(); - this->leverBras(); - usleep(500'000); + this->leverBras(); + usleep(500'000); - this->go(whiteDropPosition[0], 0); - usleep(2'000'000); + this->go(whiteDropPosition); + usleep(2'000'000); - for (int i = 0; i < 3; i++) { - if (pinceState[i] == WHITE_FLOWER) { - this->openPince(i); - usleep(1'000'000); + for (int i = 0; i < 3; i++) { + if (pinceState[i] == WHITE_FLOWER) { + this->openPince(i); + usleep(1'000'000); - pinceState[i] = NONE; - this->closePince(i); - usleep(100'000); - } - } - - for (int i = 0; i < 3; i++) { - this->middlePince(i); - } - usleep(1'000'000); - - this->setSpeed(200); - - this->go(whiteDropPosition); - awaitRobotIdle(); - - for (int i = 0; i < 3; i++) { + pinceState[i] = NONE; this->closePince(i); + usleep(100'000); } - - this->transportBras(); } + + for (int i = 0; i < 3; i++) { + this->middlePince(i); + } + usleep(1'000'000); + + this->setSpeed(200); + + this->go(whiteDropSetup); + awaitRobotIdle(); + + for (int i = 0; i < 3; i++) { + this->closePince(i); + } + + this->transportBras(); } void TCPServer::goAndTurnSolarPanel(StratPattern sp) { From 0e3912a4e6b1fd3e6ef3e74404db7101e4a9fa7c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 20:52:59 +0200 Subject: [PATCH 081/316] remove endTimeout --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index cd66ee1..1cbc315 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -429,11 +429,11 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { - auto time = std::chrono::system_clock::now(); + /*auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(82)) { this->goEnd(); return; - } + }*/ switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: From 38b78b2a3ab71143b9942eec7c52d3982e3bc60b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:12:10 +0200 Subject: [PATCH 082/316] drop white flower --- TCPServer.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 1cbc315..24d6d21 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1039,8 +1039,13 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { whiteDropPosition = std::array{775, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { - whiteDropSetup = std::array{200, 400}; - whiteDropPosition = std::array{0, 400}; + this->go(200, 400); + awaitRobotIdle(); + this->go(200, 800); + awaitRobotIdle(); + + whiteDropSetup = std::array{300, 612}; + whiteDropPosition = std::array{0, 612}; angle = -PI; } } else if (team == YELLOW) { @@ -1049,8 +1054,13 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { whiteDropPosition = std::array{2224, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { - whiteDropSetup = std::array{2800, 400}; - whiteDropPosition = std::array{3000, 400}; + this->go(2800, 400); + awaitRobotIdle(); + this->go(2800, 800); + awaitRobotIdle(); + + whiteDropSetup = std::array{1700, 612}; + whiteDropPosition = std::array{0, 612}; angle = 0; } From cc8495d3d81129c16479ced5e6d3c600d11e0238 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:21:17 +0200 Subject: [PATCH 083/316] drop white flower --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 24d6d21..b5447f4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1035,8 +1035,8 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { double angle = PI / 2; if (team == BLUE) { if (sp == DROP_WHITE_FLOWER_J1) { - whiteDropSetup = std::array{775, 300}; - whiteDropPosition = std::array{775, 0}; + whiteDropSetup = std::array{762, 300}; + whiteDropPosition = std::array{762, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { this->go(200, 400); @@ -1050,8 +1050,8 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { } } else if (team == YELLOW) { if (sp == DROP_WHITE_FLOWER_J1) { - whiteDropSetup = std::array{2224, 300}; - whiteDropPosition = std::array{2224, 0}; + whiteDropSetup = std::array{2237, 300}; + whiteDropPosition = std::array{2237, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { this->go(2800, 400); From 9edee0bfde853c7567d5142725cf0782e6ff8670 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:22:31 +0200 Subject: [PATCH 084/316] proximity --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b5447f4..0e87a2f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -781,7 +781,7 @@ std::optional TCPServer::getMostCenteredArucoTag(float borneMinX, floa bool found = false; ArucoTag mostCenteredTag = ArucoTag(); for (const auto & tag : arucoTags) { - if (tag.getNbFind() < 2) continue; + // if (tag.getNbFind() < 2) continue; if (!found) { if (tag.pos()[0] > borneMinX && tag.pos()[0] < borneMaxX && tag.pos()[1] > borneMinY && tag.pos()[1] < borneMaxY) { From 49a935d3b6054eda6f8ce646396a2298dc84178c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:30:06 +0200 Subject: [PATCH 085/316] flower --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0e87a2f..47ae8e4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -923,17 +923,17 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->arucoTags.clear(); std::optional tag = std::nullopt; - usleep(500'000); - for (int i = 0; i < 5; i++) { + usleep(200'000); + for (int i = 0; i < 4; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); - usleep(220'000); + usleep(110'000); } tag = getMostCenteredArucoTag(100, 800, -300, 300); int timeout = 0; while (!tag.has_value()) { this->broadcastMessage("strat;aruco;get aruco;1\n"); - usleep(220'000); + usleep(110'000); tag = getMostCenteredArucoTag(100, 800, -300, 300); timeout++; From 451af93754bb4f37f23ea66e92925ad6314f742b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:30:52 +0200 Subject: [PATCH 086/316] speed --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 47ae8e4..d335204 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -688,7 +688,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double thetaPrime = std::atan2(centerPlantY, centerPlantX); int previousSpeed = this->speed; - this->setSpeed(180); + this->setSpeed(190); this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); awaitRobotIdle(); @@ -1112,7 +1112,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { void TCPServer::goAndTurnSolarPanel(StratPattern sp) { int previousSpeed = this->speed; - this->setSpeed(170); + this->setSpeed(200); if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: From f0a751821dc55c9e4dcce1303616a8bb0ba93934 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:36:36 +0200 Subject: [PATCH 087/316] speed --- TCPServer.cpp | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d335204..19f17d1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -686,13 +686,12 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); - int previousSpeed = this->speed; - this->setSpeed(190); + this->setSpeed(200); + this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); awaitRobotIdle(); - this->setSpeed(200); double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX; double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY; @@ -702,7 +701,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { this->closePince(pince); usleep(500'000); - this->setSpeed(previousSpeed); + this->setSpeed(200); pinceState[pince] = TCPUtils::startWith(arucoTag.name(), "Purple_flower") ? PURPLE_FLOWER : WHITE_FLOWER; this->transportBras(); } @@ -878,19 +877,15 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->go(1000, 250); awaitRobotIdle(); - this->setSpeed(180); this->rotate(-PI/2); awaitRobotIdle(); - this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(1000, 1790); awaitRobotIdle(); - this->setSpeed(180); this->rotate(PI/2); awaitRobotIdle(); - this->setSpeed(190); } else { return; } @@ -899,19 +894,15 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->go(2000, 250); awaitRobotIdle(); - this->setSpeed(180); this->rotate(-PI/2); awaitRobotIdle(); - this->setSpeed(200); } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(2000, 1790); awaitRobotIdle(); - this->setSpeed(180); this->rotate(PI/2); awaitRobotIdle(); - this->setSpeed(200); } else { return; } @@ -1195,6 +1186,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { } void TCPServer::checkpoint(StratPattern sp) { + this->setSpeed(200); if (team == BLUE) { switch (sp) { case CHECKPOINT_BOTTOM_TO_TOP: From a39a3e3a97a9167acc7f989afed6a8d864eef9e3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:37:11 +0200 Subject: [PATCH 088/316] tiemout --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 19f17d1..353c145 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -733,10 +733,10 @@ void TCPServer::awaitRobotIdle() { usleep(100'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; - /*if (timeout > 30) { + if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); break; - }*/ + } } } From dc4b15aa78873a3235d1d08c6ee9d44b8280560f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 2 May 2024 21:41:47 +0200 Subject: [PATCH 089/316] sleep --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 353c145..2a1d1b1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -730,7 +730,7 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop while (isRobotIdle < 2) { - usleep(100'000); + usleep(200'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; if (timeout > 30) { @@ -1071,7 +1071,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { usleep(500'000); this->go(whiteDropPosition); - usleep(2'000'000); + usleep(4'000'000); for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { From 8ea8014b48c3e8c7a87c2a2dea81152d1e29ea62 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 09:36:50 +0200 Subject: [PATCH 090/316] sleep --- TCPServer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2a1d1b1..b6c4859 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1062,7 +1062,6 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { this->go(whiteDropSetup); awaitRobotIdle(); - this->setSpeed(130); this->rotate(angle); awaitRobotIdle(); @@ -1070,8 +1069,10 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { this->leverBras(); usleep(500'000); + this->setSpeed(130); + this->go(whiteDropPosition); - usleep(4'000'000); + usleep(7'000'000); for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { From 21f30485d75cc3a3c83233ad70be057bcfb5a14c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 09:37:02 +0200 Subject: [PATCH 091/316] sleep --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b6c4859..88be4aa 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -729,8 +729,8 @@ void TCPServer::awaitRobotIdle() { int timeout = 0; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop - while (isRobotIdle < 2) { - usleep(200'000); + while (isRobotIdle < 3) { + usleep(100'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; if (timeout > 30) { From cbe3b435da354ca041c1343da37274f871c04b84 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 09:45:58 +0200 Subject: [PATCH 092/316] TCP --- TCPServer.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 88be4aa..48ca347 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -122,12 +122,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::cerr << "Invalid message format, token size : " << std::to_string(tokens.size()) << " from message : " << message << std::endl; return; } - if (tokens[1] != "strat") - { - this->broadcastMessage(message, clientSocket); - } - - // EMERGENCY if (tokens[2] == "stop proximity") { if (!gameStarted) return; @@ -141,7 +135,11 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) 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); + } + // EMERGENCY else if (tokens[0] == "tirette" && tokens[2] == "set state") { this->broadcastMessage(message, clientSocket); From 8b1639214eefb741313aff9d9869461077d10954 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 10:07:02 +0200 Subject: [PATCH 093/316] position --- TCPServer.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 48ca347..d995031 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -960,9 +960,9 @@ void TCPServer::dropPurpleFlowers() { std::array purpleDropPosition{}; if (team == BLUE) { - purpleDropPosition = {225, 400}; + purpleDropPosition = {200, 400}; } else if (team == YELLOW) { - purpleDropPosition = {2775, 400}; + purpleDropPosition = {1800, 400}; } this->setSpeed(200); @@ -1019,6 +1019,8 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { return; } + this->setSpeed(200); + std::array whiteDropSetup{}; std::array whiteDropPosition{}; double angle = PI / 2; @@ -1030,7 +1032,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { } else if (sp == DROP_WHITE_FLOWER_J2) { this->go(200, 400); awaitRobotIdle(); - this->go(200, 800); + this->go(180, 800); awaitRobotIdle(); whiteDropSetup = std::array{300, 612}; @@ -1070,7 +1072,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { this->setSpeed(130); this->go(whiteDropPosition); - usleep(7'000'000); + usleep(2'000'000); for (int i = 0; i < 3; i++) { if (pinceState[i] == WHITE_FLOWER) { From ce21dcbfbf3f18fc289af792fd1f6f623f25af53 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 10:33:59 +0200 Subject: [PATCH 094/316] remove timeout --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d995031..c791fa4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -731,10 +731,10 @@ void TCPServer::awaitRobotIdle() { usleep(100'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; - if (timeout > 30) { + /*if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); break; - } + }*/ } } From 4e6fadc00ab6518b27739e2d19abcfcee10fbce9 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 11:04:39 +0200 Subject: [PATCH 095/316] exclude bad plant --- TCPServer.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index c791fa4..04e90a9 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -743,6 +743,12 @@ void TCPServer::handleArucoTag(ArucoTag &tag) { return; } + auto rotArray = tag.rot(); + + if (rotArray[2] > 0.3 && rotArray[2] < -0.3 && rotArray[0] > 3 && rotArray[0] < 2.5) { + return; + } + for (auto& t : arucoTags) { if (tag.id() == t.id()) { float tPosX = t.pos()[0]; From 2cf2a2d18a7908e98a006201e266b077833dec28 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:16:28 +0200 Subject: [PATCH 096/316] change position --- TCPServer.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 04e90a9..2ddf10a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -751,10 +751,8 @@ void TCPServer::handleArucoTag(ArucoTag &tag) { for (auto& t : arucoTags) { if (tag.id() == t.id()) { - float tPosX = t.pos()[0]; - float tPosY = t.pos()[1]; - float tagPosX = tag.pos()[0]; - float tagPosY = tag.pos()[1]; + auto [tPosX, tPosY] = t.pos(); + auto [tagPosX, tagPosY] = tag.pos(); if (tagPosX > tPosX - 10 && tagPosX < tPosX + 10 && tagPosY > tPosY - 10 && tagPosY < tPosY + 10) { t.find(); return; @@ -878,34 +876,38 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { - this->go(1000, 250); + this->go(300, 700); awaitRobotIdle(); - this->rotate(-PI/2); + this->setSpeed(180); + this->rotate(0); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1000, 1790); + this->go(300, 1300); awaitRobotIdle(); - this->rotate(PI/2); + this->setSpeed(180); + this->rotate(0); awaitRobotIdle(); } else { return; } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { - this->go(2000, 250); + this->go(1700, 700); awaitRobotIdle(); - this->rotate(-PI/2); + this->setSpeed(180); + this->rotate(-PI); awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(2000, 1790); + this->go(1700, 1300); awaitRobotIdle(); - this->rotate(PI/2); + this->setSpeed(180); + this->rotate(-PI); awaitRobotIdle(); } else { return; @@ -914,22 +916,20 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { return; } - this->arucoTags.clear(); std::optional tag = std::nullopt; - usleep(200'000); for (int i = 0; i < 4; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); } - tag = getMostCenteredArucoTag(100, 800, -300, 300); + tag = getMostCenteredArucoTag(100, 800, -200, 200); int timeout = 0; while (!tag.has_value()) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); - tag = getMostCenteredArucoTag(100, 800, -300, 300); + tag = getMostCenteredArucoTag(100, 800, -200, 200); timeout++; if (timeout > 3) { From 5ae399eaa41587d0e5599208f1a86b79476ad5e1 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:17:58 +0200 Subject: [PATCH 097/316] c++ --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2ddf10a..174aa74 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -650,8 +650,9 @@ void TCPServer::startGameTest() { void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { - double robotPosX = this->robotPose.pos.x; - double robotPosY = this->robotPose.pos.y; + + auto [robotPosX, robotPosY] = this->robotPose.pos; + double theta = this->robotPose.theta; double decalage; if (pince < 0 || pince > 2) { @@ -676,8 +677,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { this->baisserBras(); this->openPince(pince); - double xPrime = arucoTag.pos()[0]; - double yPrime = arucoTag.pos()[1]; + auto [xPrime, yPrime] = arucoTag.pos(); double roll = arucoTag.rot()[1]; auto centerPlantX = (20 * std::cos(roll)) + xPrime; From 3585901f843b2394706d222b06e52aa47b048fa3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:23:02 +0200 Subject: [PATCH 098/316] pos --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 174aa74..0190c8e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -884,6 +884,8 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { + this->go(500, 1800); + awaitRobotIdle(); this->go(300, 1300); awaitRobotIdle(); From 538c0ab7b55b34889a3b5492034883036d8d6c4a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:26:03 +0200 Subject: [PATCH 099/316] checkpoint --- TCPServer.cpp | 16 ++++++++++++---- TCPServer.h | 4 +++- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0190c8e..b4aaeba 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1210,23 +1210,31 @@ void TCPServer::checkpoint(StratPattern sp) { this->go(500, 1500); this->awaitRobotIdle(); break; + case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: + this->go(500, 1700); + awaitRobotIdle(); + break; default: break; } } else if (team == YELLOW) { switch (sp) { case CHECKPOINT_BOTTOM_TO_TOP: - this->go(1500, 1500); + this->go(2500, 1500); this->awaitRobotIdle(); - this->go(1500, 500); + this->go(2500, 500); this->awaitRobotIdle(); break; case CHECKPOINT_TOP_TO_BOTTOM: - this->go(1500, 500); + this->go(2500, 500); this->awaitRobotIdle(); - this->go(1500, 1500); + this->go(2500, 1500); this->awaitRobotIdle(); break; + case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: + this->go(2500, 1700); + awaitRobotIdle(); + break; default: break; } diff --git a/TCPServer.h b/TCPServer.h index 86963ad..e772f9b 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -46,7 +46,8 @@ enum StratPattern { GO_END, GET_LIDAR_POS, CHECKPOINT_BOTTOM_TO_TOP, - CHECKPOINT_TOP_TO_BOTTOM + CHECKPOINT_TOP_TO_BOTTOM, + CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, }; class TCPServer; // Forward declaration @@ -106,6 +107,7 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, + CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, From 27cf512f69fbb01acab7fadfc8a59272461046d5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:28:58 +0200 Subject: [PATCH 100/316] checkpoint --- TCPServer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b4aaeba..1a06320 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -470,6 +470,9 @@ void TCPServer::startGame() { case CHECKPOINT_TOP_TO_BOTTOM: checkpoint(CHECKPOINT_TOP_TO_BOTTOM); break; + case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: + checkpoint(CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER); + break; } whereAmI++; } @@ -884,8 +887,6 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(500, 1800); - awaitRobotIdle(); this->go(300, 1300); awaitRobotIdle(); From 1b06026fb4ad1c227c45180e4959f1b78741b0ed Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:33:25 +0200 Subject: [PATCH 101/316] borne --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 1a06320..3d9ea4c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -926,13 +926,13 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); } - tag = getMostCenteredArucoTag(100, 800, -200, 200); + tag = getMostCenteredArucoTag(300, 1000, -200, 200); int timeout = 0; while (!tag.has_value()) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); - tag = getMostCenteredArucoTag(100, 800, -200, 200); + tag = getMostCenteredArucoTag(300, 1000, -200, 200); timeout++; if (timeout > 3) { From 121bde84e32305346d89e3b3137bb084c87239a3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:40:32 +0200 Subject: [PATCH 102/316] checkpoint --- TCPServer.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3d9ea4c..6040d5f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1041,7 +1041,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { } else if (sp == DROP_WHITE_FLOWER_J2) { this->go(200, 400); awaitRobotIdle(); - this->go(180, 800); + this->go(180, 1000); awaitRobotIdle(); whiteDropSetup = std::array{300, 612}; @@ -1200,14 +1200,10 @@ void TCPServer::checkpoint(StratPattern sp) { if (team == BLUE) { switch (sp) { case CHECKPOINT_BOTTOM_TO_TOP: - this->go(500, 1500); - this->awaitRobotIdle(); this->go(500, 500); this->awaitRobotIdle(); break; case CHECKPOINT_TOP_TO_BOTTOM: - this->go(500, 500); - this->awaitRobotIdle(); this->go(500, 1500); this->awaitRobotIdle(); break; @@ -1221,14 +1217,10 @@ void TCPServer::checkpoint(StratPattern sp) { } else if (team == YELLOW) { switch (sp) { case CHECKPOINT_BOTTOM_TO_TOP: - this->go(2500, 1500); - this->awaitRobotIdle(); this->go(2500, 500); this->awaitRobotIdle(); break; case CHECKPOINT_TOP_TO_BOTTOM: - this->go(2500, 500); - this->awaitRobotIdle(); this->go(2500, 1500); this->awaitRobotIdle(); break; From 3beb77c6a38cac9733fd91fafd140a6d31c3766a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:42:40 +0200 Subject: [PATCH 103/316] borne --- TCPServer.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6040d5f..2e15043 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -787,13 +787,17 @@ std::optional TCPServer::getMostCenteredArucoTag(float borneMinX, floa for (const auto & tag : arucoTags) { // if (tag.getNbFind() < 2) continue; + auto [tagX, tagY] = tag.pos(); + if (!found) { - if (tag.pos()[0] > borneMinX && tag.pos()[0] < borneMaxX && tag.pos()[1] > borneMinY && tag.pos()[1] < borneMaxY) { + if (tagX > borneMinX && tagX < borneMaxX && tagY > borneMinY && tagY < borneMaxY) { mostCenteredTag = tag; found = true; } } else if (distanceToTag(tag) < distanceToTag(mostCenteredTag)) { - mostCenteredTag = tag; + if (tagX > borneMinX && tagX < borneMaxX && tagY > borneMinY && tagY < borneMaxY) { + mostCenteredTag = tag; + } } } From df9508090facadc3abb50ce8f6a96c799038dd8e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:43:32 +0200 Subject: [PATCH 104/316] -20 --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2e15043..1ab7819 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -683,7 +683,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { auto [xPrime, yPrime] = arucoTag.pos(); double roll = arucoTag.rot()[1]; - auto centerPlantX = (20 * std::cos(roll)) + xPrime; + auto centerPlantX = (20 * std::cos(roll)) + xPrime - 20; auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); From b5a1dca2fd93e761c4264fb9e597bbc71447a4fa Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:49:53 +0200 Subject: [PATCH 105/316] plant --- TCPServer.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 1ab7819..8f15d59 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -883,7 +883,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->setSpeed(200); if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { - this->go(300, 700); + this->go(500, 700); awaitRobotIdle(); this->setSpeed(180); @@ -891,7 +891,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(300, 1300); + this->go(500, 1300); awaitRobotIdle(); this->setSpeed(180); @@ -902,7 +902,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { - this->go(1700, 700); + this->go(1500, 700); awaitRobotIdle(); this->setSpeed(180); @@ -910,7 +910,7 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { awaitRobotIdle(); } else if (sp == TAKE_FLOWER_BOTTOM) { - this->go(1700, 1300); + this->go(1500, 1300); awaitRobotIdle(); this->setSpeed(180); @@ -1204,6 +1204,8 @@ void TCPServer::checkpoint(StratPattern sp) { if (team == BLUE) { switch (sp) { case CHECKPOINT_BOTTOM_TO_TOP: + this->go(500, 1000); + awaitRobotIdle(); this->go(500, 500); this->awaitRobotIdle(); break; @@ -1221,6 +1223,8 @@ void TCPServer::checkpoint(StratPattern sp) { } else if (team == YELLOW) { switch (sp) { case CHECKPOINT_BOTTOM_TO_TOP: + this->go(2500, 1000); + awaitRobotIdle(); this->go(2500, 500); this->awaitRobotIdle(); break; From 8047d8942409eb7555d421a7e0aca89722cec124 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:51:24 +0200 Subject: [PATCH 106/316] drop white flower --- TCPServer.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8f15d59..1040243 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1047,6 +1047,8 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { awaitRobotIdle(); this->go(180, 1000); awaitRobotIdle(); + this->go(220, 650); + awaitRobotIdle(); whiteDropSetup = std::array{300, 612}; whiteDropPosition = std::array{0, 612}; @@ -1062,6 +1064,8 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { awaitRobotIdle(); this->go(2800, 800); awaitRobotIdle(); + this->go(1780, 650); + awaitRobotIdle(); whiteDropSetup = std::array{1700, 612}; whiteDropPosition = std::array{0, 612}; From 9fc41f51850831b69ac46fe91d08c6876ea74a4c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:52:10 +0200 Subject: [PATCH 107/316] speed when rotating --- TCPServer.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 1040243..6ba0939 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -731,7 +731,7 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop while (isRobotIdle < 3) { - usleep(100'000); + usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; /*if (timeout > 30) { @@ -886,7 +886,6 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->go(500, 700); awaitRobotIdle(); - this->setSpeed(180); this->rotate(0); awaitRobotIdle(); } @@ -894,7 +893,6 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->go(500, 1300); awaitRobotIdle(); - this->setSpeed(180); this->rotate(0); awaitRobotIdle(); } else { @@ -905,7 +903,6 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->go(1500, 700); awaitRobotIdle(); - this->setSpeed(180); this->rotate(-PI); awaitRobotIdle(); } @@ -913,7 +910,6 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->go(1500, 1300); awaitRobotIdle(); - this->setSpeed(180); this->rotate(-PI); awaitRobotIdle(); } else { From 390aad101979a2fe52630b09c0b7c3ad628bb745 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 12:58:53 +0200 Subject: [PATCH 108/316] white flower --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6ba0939..3c4f08a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1041,7 +1041,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { } else if (sp == DROP_WHITE_FLOWER_J2) { this->go(200, 400); awaitRobotIdle(); - this->go(180, 1000); + this->go(200, 1000); awaitRobotIdle(); this->go(220, 650); awaitRobotIdle(); @@ -1058,7 +1058,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { } else if (sp == DROP_WHITE_FLOWER_J2) { this->go(2800, 400); awaitRobotIdle(); - this->go(2800, 800); + this->go(2800, 1000); awaitRobotIdle(); this->go(1780, 650); awaitRobotIdle(); From 4ab248d8129ae96470be7c17abdb788d20978f84 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:07:30 +0200 Subject: [PATCH 109/316] sleep --- TCPServer.cpp | 38 ++++++++++++++++++++++---------------- TCPServer.h | 2 ++ 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3c4f08a..d02aa17 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1039,12 +1039,15 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { whiteDropPosition = std::array{762, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { - this->go(200, 400); - awaitRobotIdle(); - this->go(200, 1000); - awaitRobotIdle(); - this->go(220, 650); - awaitRobotIdle(); + if (!potJardiniere2Removed) { + this->go(200, 400); + awaitRobotIdle(); + this->go(200, 900); + awaitRobotIdle(); + this->go(220, 650); + awaitRobotIdle(); + potJardiniere2Removed = true; + } whiteDropSetup = std::array{300, 612}; whiteDropPosition = std::array{0, 612}; @@ -1056,12 +1059,15 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { whiteDropPosition = std::array{2237, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { - this->go(2800, 400); - awaitRobotIdle(); - this->go(2800, 1000); - awaitRobotIdle(); - this->go(1780, 650); - awaitRobotIdle(); + if (!potJardiniere2Removed) { + this->go(2800, 400); + awaitRobotIdle(); + this->go(2800, 900); + awaitRobotIdle(); + this->go(1780, 650); + awaitRobotIdle(); + potJardiniere2Removed = true; + } whiteDropSetup = std::array{1700, 612}; whiteDropPosition = std::array{0, 612}; @@ -1207,7 +1213,7 @@ void TCPServer::checkpoint(StratPattern sp) { this->go(500, 1000); awaitRobotIdle(); this->go(500, 500); - this->awaitRobotIdle(); + usleep(800'000); break; case CHECKPOINT_TOP_TO_BOTTOM: this->go(500, 1500); @@ -1215,7 +1221,7 @@ void TCPServer::checkpoint(StratPattern sp) { break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(500, 1700); - awaitRobotIdle(); + usleep(800'000); break; default: break; @@ -1226,7 +1232,7 @@ void TCPServer::checkpoint(StratPattern sp) { this->go(2500, 1000); awaitRobotIdle(); this->go(2500, 500); - this->awaitRobotIdle(); + usleep(8500'000); break; case CHECKPOINT_TOP_TO_BOTTOM: this->go(2500, 1500); @@ -1234,7 +1240,7 @@ void TCPServer::checkpoint(StratPattern sp) { break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(2500, 1700); - awaitRobotIdle(); + usleep(800'000); break; default: break; diff --git a/TCPServer.h b/TCPServer.h index e772f9b..3e49986 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -144,6 +144,8 @@ private: int lidarSocket = -1; int arduinoSocket = -1; + bool potJardiniere2Removed = false; + public: explicit TCPServer(int port); From 01b34ab3d91a2042e8771d1d5026027686b73827 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:07:36 +0200 Subject: [PATCH 110/316] sleep --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d02aa17..5f6c5d1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1232,7 +1232,7 @@ void TCPServer::checkpoint(StratPattern sp) { this->go(2500, 1000); awaitRobotIdle(); this->go(2500, 500); - usleep(8500'000); + usleep(800'000); break; case CHECKPOINT_TOP_TO_BOTTOM: this->go(2500, 1500); From 85408fbe26787e27b67bd46a9926c9d426ede813 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:08:17 +0200 Subject: [PATCH 111/316] more strat --- TCPServer.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 3e49986..11ff03a 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -124,10 +124,11 @@ private: DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2, // GET_LIDAR_POS, - /* TAKE_FLOWER_TOP, + TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, - DROP_FLOWER, */ + DROP_WHITE_FLOWER_J2, + DROP_PURPLE_FLOWER, GO_END }; From e8ad272552b315a81b08ae10d637a59ed17b0382 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:13:42 +0200 Subject: [PATCH 112/316] remove fake --- TCPServer.cpp | 2 +- TCPServer.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5f6c5d1..636dcea 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -785,7 +785,7 @@ std::optional TCPServer::getMostCenteredArucoTag(float borneMinX, floa bool found = false; ArucoTag mostCenteredTag = ArucoTag(); for (const auto & tag : arucoTags) { - // if (tag.getNbFind() < 2) continue; + if (tag.getNbFind() < 2) continue; auto [tagX, tagY] = tag.pos(); diff --git a/TCPServer.h b/TCPServer.h index 11ff03a..276a4bc 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -124,11 +124,11 @@ private: DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2, // GET_LIDAR_POS, - TAKE_FLOWER_TOP, + /*TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, DROP_WHITE_FLOWER_J2, - DROP_PURPLE_FLOWER, + DROP_PURPLE_FLOWER,*/ GO_END }; From dae87ca10f0e623b36aeafe54d8533ad063fa3f6 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:15:14 +0200 Subject: [PATCH 113/316] timeout --- TCPServer.cpp | 8 ++++---- TCPServer.h | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 636dcea..bcd824d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -427,11 +427,11 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { - /*auto time = std::chrono::system_clock::now(); + auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(82)) { this->goEnd(); return; - }*/ + } switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: @@ -734,10 +734,10 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; - /*if (timeout > 30) { + if (timeout > 30) { this->broadcastMessage("strat;arduino;clear;1"); break; - }*/ + } } } diff --git a/TCPServer.h b/TCPServer.h index 276a4bc..e32fec7 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -124,11 +124,11 @@ private: DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2, // GET_LIDAR_POS, - /*TAKE_FLOWER_TOP, + TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, DROP_WHITE_FLOWER_J2, - DROP_PURPLE_FLOWER,*/ + DROP_PURPLE_FLOWER, GO_END }; From 96c9aabfbe9bf1950727ad010b44416d21a6c9fa Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:18:17 +0200 Subject: [PATCH 114/316] timeout --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index bcd824d..c40690e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -734,7 +734,7 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; - if (timeout > 30) { + if (timeout > 100) { this->broadcastMessage("strat;arduino;clear;1"); break; } From 35388419bbae9fbb7685806b4a865aaa4d427960 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:29:57 +0200 Subject: [PATCH 115/316] distance to plant --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c40690e..430d4f8 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -734,7 +734,7 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; - if (timeout > 100) { + if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; } @@ -922,17 +922,17 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { this->arucoTags.clear(); std::optional tag = std::nullopt; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 5; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); } - tag = getMostCenteredArucoTag(300, 1000, -200, 200); + tag = getMostCenteredArucoTag(300, 700, -200, 200); int timeout = 0; while (!tag.has_value()) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); - tag = getMostCenteredArucoTag(300, 1000, -200, 200); + tag = getMostCenteredArucoTag(300, 700, -200, 200); timeout++; if (timeout > 3) { From 693408cf23db60467fe1e2af2ae0e9c7f58cd131 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 13:32:21 +0200 Subject: [PATCH 116/316] sleep --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 430d4f8..aa08106 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1213,7 +1213,7 @@ void TCPServer::checkpoint(StratPattern sp) { this->go(500, 1000); awaitRobotIdle(); this->go(500, 500); - usleep(800'000); + usleep(500'000); break; case CHECKPOINT_TOP_TO_BOTTOM: this->go(500, 1500); @@ -1221,7 +1221,7 @@ void TCPServer::checkpoint(StratPattern sp) { break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(500, 1700); - usleep(800'000); + usleep(500'000); break; default: break; @@ -1232,7 +1232,7 @@ void TCPServer::checkpoint(StratPattern sp) { this->go(2500, 1000); awaitRobotIdle(); this->go(2500, 500); - usleep(800'000); + usleep(500'000); break; case CHECKPOINT_TOP_TO_BOTTOM: this->go(2500, 1500); @@ -1240,7 +1240,7 @@ void TCPServer::checkpoint(StratPattern sp) { break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(2500, 1700); - usleep(800'000); + usleep(500'000); break; default: break; From 6009a894625975f7d1c93a13cf685bfdf09d79e2 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 14:47:14 +0200 Subject: [PATCH 117/316] pince --- TCPServer.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index aa08106..9167fe7 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -876,6 +876,11 @@ void TCPServer::goEnd() { this->rotate(this->endRobotPose.theta); awaitRobotIdle(); + for (int i = 0 ; i < 3; i++) { + this->middlePince(i); + usleep(50'000); + } + this->broadcastMessage("strat;all;end;1"); } @@ -941,12 +946,19 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { } if (tag.has_value()) { - if (pinceState[1] == NONE) { + /*if (pinceState[1] == NONE) { goToAruco(tag.value(), 1); } else if (pinceState[2] == NONE) { goToAruco(tag.value(), 2); } else if (pinceState[0] == NONE) { goToAruco(tag.value(), 0); + }*/ + if (pinceState[0] == NONE) { + goToAruco(tag.value(), 0); + } else if (pinceState[2] == NONE) { + goToAruco(tag.value(), 2); + } else if (pinceState[1] == NONE) { + goToAruco(tag.value(), 1); } } } From 93a3cff323e39da4476ffb8a61da0202828ba4d8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 15:00:53 +0200 Subject: [PATCH 118/316] points --- TCPServer.cpp | 13 +++++++++++++ TCPServer.h | 2 ++ 2 files changed, 15 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9167fe7..d767deb 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -880,8 +880,11 @@ void TCPServer::goEnd() { this->middlePince(i); usleep(50'000); } + this->sendPoint(10); this->broadcastMessage("strat;all;end;1"); + this->sendPoint(10); + this->sendPoint(5); } void TCPServer::findAndGoFlower(const StratPattern sp) { @@ -1012,6 +1015,8 @@ void TCPServer::dropPurpleFlowers() { pinceState[toDrop] = NONE; this->closePince(toDrop); usleep(200'000); + + this->sendPoint(3); } } @@ -1113,6 +1118,8 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { pinceState[i] = NONE; this->closePince(i); usleep(100'000); + + this->sendPoint(4); } } @@ -1214,6 +1221,8 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { } } + this->sendPoint(5); + this->setSpeed(previousSpeed); } @@ -1383,3 +1392,7 @@ void TCPServer::uncheckPanneau(int servo_moteur) { void TCPServer::askLidarPosition() { this->broadcastMessage("start;lidar;get pos;1\n"); } + +void TCPServer::sendPoint(int point) { + this->broadcastMessage("strat;ihm;add point;" + std::to_string(point) + "\n"); +} diff --git a/TCPServer.h b/TCPServer.h index e32fec7..9ac90a7 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -268,5 +268,7 @@ public: void askLidarPosition(); + void sendPoint(int point); + ~TCPServer(); }; From 4e19bef4f3971a17cb975c2041aa380a9e6cf87e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 15:35:19 +0200 Subject: [PATCH 119/316] points | pince --- TCPServer.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d767deb..9bb9d63 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -664,13 +664,13 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { switch (pince) { case 0: - decalage = 60; + decalage = 85; break; case 1: decalage = 0; break; case 2: - decalage = -60; + decalage = -85; break; default: decalage = 0; @@ -683,7 +683,7 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { auto [xPrime, yPrime] = arucoTag.pos(); double roll = arucoTag.rot()[1]; - auto centerPlantX = (20 * std::cos(roll)) + xPrime - 20; + auto centerPlantX = (20 * std::cos(roll)) + xPrime - 20 + (decalage / 10); auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); @@ -883,8 +883,6 @@ void TCPServer::goEnd() { this->sendPoint(10); this->broadcastMessage("strat;all;end;1"); - this->sendPoint(10); - this->sendPoint(5); } void TCPServer::findAndGoFlower(const StratPattern sp) { From 22a494f2adb1717381ba6c3679a0fe6b2d805bd7 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 16:00:34 +0200 Subject: [PATCH 120/316] const --- TCPServer.cpp | 28 +++++++++++++++------------- TCPServer.h | 5 +---- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9bb9d63..483abfa 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -287,7 +287,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } else if (tokens[2] == "set pos") { std::vector pos = TCPUtils::split(tokens[3], ","); this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; - this->setPosition(this->robotPose, lidarSocket); + if (!awaitForLidar) { + this->setPosition(this->robotPose, lidarSocket); + } } } else if (tokens[2] == "test aruco") { int pince = std::stoi(tokens[3]); @@ -664,13 +666,13 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { switch (pince) { case 0: - decalage = 85; + decalage = 75; break; case 1: decalage = 0; break; case 2: - decalage = -85; + decalage = -75; break; default: decalage = 0; @@ -741,7 +743,7 @@ void TCPServer::awaitRobotIdle() { } } -void TCPServer::handleArucoTag(ArucoTag &tag) { +void TCPServer::handleArucoTag(const ArucoTag &tag) { if (!TCPUtils::contains(tag.name(), "flower")) { return; } @@ -766,8 +768,8 @@ void TCPServer::handleArucoTag(ArucoTag &tag) { this->arucoTags.push_back(tag); } -std::optional TCPServer::getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, - float borneMaxY) { +std::optional TCPServer::getBiggestArucoTag(const float borneMinX, const float borneMaxX, const float borneMinY, + const float borneMaxY) { bool found = false; ArucoTag biggestTag = ArucoTag(); for (const auto & tag : arucoTags) { @@ -781,7 +783,7 @@ std::optional TCPServer::getBiggestArucoTag(float borneMinX, float bor } -std::optional TCPServer::getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY) { +std::optional TCPServer::getMostCenteredArucoTag(const float borneMinX, const float borneMaxX, const float borneMinY, const float borneMaxY) { bool found = false; ArucoTag mostCenteredTag = ArucoTag(); for (const auto & tag : arucoTags) { @@ -827,7 +829,7 @@ void TCPServer::handleEmergency(int distance, double angle) { this->gameThread.detach(); } -void TCPServer::startTestAruco(int pince) { +void TCPServer::startTestAruco(const int pince) { this->arucoTags.clear(); std::optional tag = std::nullopt; @@ -1000,7 +1002,7 @@ void TCPServer::dropPurpleFlowers() { this->baisserBras(); - for (auto & toDrop : pinceHavePurpleFlower) { + for (const auto & toDrop : pinceHavePurpleFlower) { this->openPince(toDrop); usleep(200'000); @@ -1023,7 +1025,7 @@ void TCPServer::dropPurpleFlowers() { this->setSpeed(200); } -void TCPServer::dropWhiteFlowers(StratPattern sp) { +void TCPServer::dropWhiteFlowers(const StratPattern sp) { std::vector pinceHaveWhiteFlower; pinceHaveWhiteFlower.reserve(3); @@ -1138,7 +1140,7 @@ void TCPServer::dropWhiteFlowers(StratPattern sp) { this->transportBras(); } -void TCPServer::goAndTurnSolarPanel(StratPattern sp) { +void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { int previousSpeed = this->speed; this->setSpeed(200); if (team == BLUE) { @@ -1224,7 +1226,7 @@ void TCPServer::goAndTurnSolarPanel(StratPattern sp) { this->setSpeed(previousSpeed); } -void TCPServer::checkpoint(StratPattern sp) { +void TCPServer::checkpoint(const StratPattern sp) { this->setSpeed(200); if (team == BLUE) { switch (sp) { @@ -1276,7 +1278,7 @@ void TCPServer::getLidarPos() { // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop while (awaitForLidar) { - usleep(200'000); + usleep(100'000); } // ReSharper disable once CppDFAUnreachableCode diff --git a/TCPServer.h b/TCPServer.h index 9ac90a7..48f1e16 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -112,15 +112,12 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, CHECKPOINT_BOTTOM_TO_TOP, - // DROP_FLOWER, DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1, // GET_LIDAR_POS, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, - // GET_LIDAR_POS, - // DROP_FLOWER, DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2, // GET_LIDAR_POS, @@ -186,7 +183,7 @@ public: void awaitRobotIdle(); - void handleArucoTag(ArucoTag &tag); + void handleArucoTag(const ArucoTag &tag); std::optional getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); From 8d4aef9f5bbeb03c0c4e9e053c414cf461b4a36a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 17:31:47 +0200 Subject: [PATCH 121/316] new start --- TCPServer.cpp | 133 +++++++++++++++++++++++++++++++++++++++++++++++++- TCPServer.h | 37 +++++++++----- utils.h | 1 + 3 files changed, 157 insertions(+), 14 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 483abfa..22f67bc 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -475,6 +475,18 @@ void TCPServer::startGame() { case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: checkpoint(CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER); break; + case DROP_FLOWER_J1: + dropFlowers(DROP_FLOWER_J1); + break; + case DROP_FLOWER_J2: + dropFlowers(DROP_WHITE_FLOWER_J2); + break; + case TAKE_3_PLANT_TOP: + go3Plants(TAKE_3_PLANT_TOP); + break; + case TAKE_3_PLANT_BOTTOM: + go3Plants(TAKE_3_PLANT_BOTTOM); + break; } whereAmI++; } @@ -1035,8 +1047,7 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) { case WHITE_FLOWER: pinceHaveWhiteFlower.push_back(i); break; - case PURPLE_FLOWER: - case NONE: + default: break; } } @@ -1269,6 +1280,124 @@ void TCPServer::checkpoint(const StratPattern sp) { } } +void TCPServer::dropFlowers(StratPattern sp) { + + std::array whiteDropSetup{}; + std::array whiteDropPosition{}; + double angle = PI / 2; + if (team == BLUE) { + if (sp == DROP_FLOWER_J1) { + whiteDropSetup = std::array{762, 300}; + whiteDropPosition = std::array{762, 0}; + angle = PI / 2; + } else if (sp == DROP_FLOWER_J2) { + if (!potJardiniere2Removed) { + this->go(200, 400); + awaitRobotIdle(); + this->go(200, 900); + awaitRobotIdle(); + this->go(220, 650); + awaitRobotIdle(); + potJardiniere2Removed = true; + } + + whiteDropSetup = std::array{300, 612}; + whiteDropPosition = std::array{0, 612}; + angle = -PI; + } + } else if (team == YELLOW) { + if (sp == DROP_FLOWER_J1) { + whiteDropSetup = std::array{2237, 300}; + whiteDropPosition = std::array{2237, 0}; + angle = PI / 2; + } else if (sp == DROP_FLOWER_J2) { + if (!potJardiniere2Removed) { + this->go(2800, 400); + awaitRobotIdle(); + this->go(2800, 900); + awaitRobotIdle(); + this->go(1780, 650); + awaitRobotIdle(); + potJardiniere2Removed = true; + } + + whiteDropSetup = std::array{1700, 612}; + whiteDropPosition = std::array{0, 612}; + angle = 0; + } + } + + this->setSpeed(200); + + this->go(whiteDropSetup); + awaitRobotIdle(); + + + this->rotate(angle); + awaitRobotIdle(); + + this->leverBras(); + usleep(500'000); + + this->setSpeed(130); + + this->go(whiteDropPosition); + usleep(2'000'000); + + for (int i = 0; i < 3; i++) { + this->openPince(i); + pinceState[i] = NONE; + this->sendPoint(4); + } + usleep(1'000'000); + + this->setSpeed(200); + + this->go(whiteDropSetup); + awaitRobotIdle(); + + for (int i = 0; i < 3; i++) { + this->closePince(i); + } + + this->transportBras(); +} + +void TCPServer::go3Plants(StratPattern sp) { + if (sp == TAKE_FLOWER_TOP) { + this->go(500, 700); + awaitRobotIdle(); + + this->rotate(0); + awaitRobotIdle(); + } + else if (sp == TAKE_FLOWER_BOTTOM) { + this->go(500, 1300); + awaitRobotIdle(); + + this->rotate(0); + awaitRobotIdle(); + } else { + return; + } + + for (int i = 0; i < 3; i++) { + this->openPince(i); + } + usleep(200'000); + + this->go(900, 1300); + awaitRobotIdle(); + + for (int i = 0; i < 3; i++) { + this->closePince(i); + pinceState[i] = FLOWER; + } + usleep(200'000); + + this->go(800, 1300); + awaitRobotIdle(); +} void TCPServer::getLidarPos() { diff --git a/TCPServer.h b/TCPServer.h index 48f1e16..0368994 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -48,6 +48,10 @@ enum StratPattern { CHECKPOINT_BOTTOM_TO_TOP, CHECKPOINT_TOP_TO_BOTTOM, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + TAKE_3_PLANT_BOTTOM, + TAKE_3_PLANT_TOP, + DROP_FLOWER_J1, + DROP_FLOWER_J2 }; class TCPServer; // Forward declaration @@ -108,24 +112,29 @@ private: TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, - TAKE_FLOWER_BOTTOM, - TAKE_FLOWER_BOTTOM, + TAKE_FLOWER_BOTTOM,*/ + TAKE_3_PLANT_BOTTOM, + DROP_FLOWER_J1, CHECKPOINT_BOTTOM_TO_TOP, - DROP_PURPLE_FLOWER, - DROP_WHITE_FLOWER_J1, + /*DROP_PURPLE_FLOWER, + DROP_WHITE_FLOWER_J1,*/ // GET_LIDAR_POS, + /*TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, - TAKE_FLOWER_TOP, - TAKE_FLOWER_TOP, - DROP_PURPLE_FLOWER, - DROP_WHITE_FLOWER_J2, + TAKE_FLOWER_TOP,*/ + TAKE_3_PLANT_TOP, + DROP_FLOWER_J2, + /*DROP_PURPLE_FLOWER, + DROP_WHITE_FLOWER_J2,*/ // GET_LIDAR_POS, + /*TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, - TAKE_FLOWER_TOP, - TAKE_FLOWER_TOP, - DROP_WHITE_FLOWER_J2, - DROP_PURPLE_FLOWER, + TAKE_FLOWER_TOP,*/ + //TAKE_3_PLANT_TOP, + /*DROP_WHITE_FLOWER_J2, + DROP_PURPLE_FLOWER,*/ GO_END }; @@ -207,6 +216,10 @@ public: void getLidarPos(); void checkpoint(StratPattern sp); + + void dropFlowers(StratPattern sp); + + void go3Plants(StratPattern sp); /* * End Strategy function */ diff --git a/utils.h b/utils.h index 0563e5a..ce106bc 100644 --- a/utils.h +++ b/utils.h @@ -12,6 +12,7 @@ enum PinceState { WHITE_FLOWER, PURPLE_FLOWER, + FLOWER, NONE }; From fb944cf1fe3227f899f52d60db330299c4e3ed9a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 17:35:20 +0200 Subject: [PATCH 122/316] new start --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 22f67bc..5519562 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1364,14 +1364,14 @@ void TCPServer::dropFlowers(StratPattern sp) { } void TCPServer::go3Plants(StratPattern sp) { - if (sp == TAKE_FLOWER_TOP) { + if (sp == TAKE_3_PLANT_TOP) { this->go(500, 700); awaitRobotIdle(); this->rotate(0); awaitRobotIdle(); } - else if (sp == TAKE_FLOWER_BOTTOM) { + else if (sp == TAKE_3_PLANT_BOTTOM) { this->go(500, 1300); awaitRobotIdle(); From 18b2a2d9122530ff45e9b55ee05e259b357b07c4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 17:54:05 +0200 Subject: [PATCH 123/316] new start --- TCPServer.cpp | 45 +++++++++++++++++++++++++-------------------- TCPServer.h | 16 ++++++++++------ 2 files changed, 35 insertions(+), 26 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5519562..e8e1d61 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -487,6 +487,9 @@ void TCPServer::startGame() { case TAKE_3_PLANT_BOTTOM: go3Plants(TAKE_3_PLANT_BOTTOM); break; + case REMOVE_POT_J2: + removePot(REMOVE_POT_J2); + break; } whereAmI++; } @@ -1291,16 +1294,6 @@ void TCPServer::dropFlowers(StratPattern sp) { whiteDropPosition = std::array{762, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - if (!potJardiniere2Removed) { - this->go(200, 400); - awaitRobotIdle(); - this->go(200, 900); - awaitRobotIdle(); - this->go(220, 650); - awaitRobotIdle(); - potJardiniere2Removed = true; - } - whiteDropSetup = std::array{300, 612}; whiteDropPosition = std::array{0, 612}; angle = -PI; @@ -1311,16 +1304,6 @@ void TCPServer::dropFlowers(StratPattern sp) { whiteDropPosition = std::array{2237, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - if (!potJardiniere2Removed) { - this->go(2800, 400); - awaitRobotIdle(); - this->go(2800, 900); - awaitRobotIdle(); - this->go(1780, 650); - awaitRobotIdle(); - potJardiniere2Removed = true; - } - whiteDropSetup = std::array{1700, 612}; whiteDropPosition = std::array{0, 612}; angle = 0; @@ -1399,6 +1382,28 @@ void TCPServer::go3Plants(StratPattern sp) { awaitRobotIdle(); } +void TCPServer::removePot(StratPattern sp) { + if (team == BLUE) { + if (sp == REMOVE_POT_J2) { + this->go(230, 1000); + awaitRobotIdle(); + this->go(200, 500); + awaitRobotIdle(); + this->go(220, 650); + awaitRobotIdle(); + } + } else if (team == YELLOW) { + if (sp == REMOVE_POT_J2) { + this->go(2770, 1000); + awaitRobotIdle(); + this->go(2800, 500); + awaitRobotIdle(); + this->go(2780, 650); + awaitRobotIdle(); + } + } +} + void TCPServer::getLidarPos() { this->askLidarPosition(); diff --git a/TCPServer.h b/TCPServer.h index 0368994..6cacfb9 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -51,7 +51,9 @@ enum StratPattern { TAKE_3_PLANT_BOTTOM, TAKE_3_PLANT_TOP, DROP_FLOWER_J1, - DROP_FLOWER_J2 + DROP_FLOWER_J2, + REMOVE_POT_J2, + DROP_FLOWER_BASE_1, }; class TCPServer; // Forward declaration @@ -116,8 +118,8 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ TAKE_3_PLANT_BOTTOM, - DROP_FLOWER_J1, - CHECKPOINT_BOTTOM_TO_TOP, + REMOVE_POT_J2, + DROP_FLOWER_J2, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ // GET_LIDAR_POS, @@ -125,7 +127,7 @@ private: TAKE_FLOWER_TOP, TAKE_FLOWER_TOP,*/ TAKE_3_PLANT_TOP, - DROP_FLOWER_J2, + DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2,*/ // GET_LIDAR_POS, @@ -135,6 +137,8 @@ private: //TAKE_3_PLANT_TOP, /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ + // TAKE_3_PLANT_TOP, + // DROP_FLOWER_BASE_1, GO_END }; @@ -151,8 +155,6 @@ private: int lidarSocket = -1; int arduinoSocket = -1; - bool potJardiniere2Removed = false; - public: explicit TCPServer(int port); @@ -220,6 +222,8 @@ public: void dropFlowers(StratPattern sp); void go3Plants(StratPattern sp); + + void removePot(StratPattern sp); /* * End Strategy function */ From b2b9b82c85b9fa78e988b908f7e5592bba151b23 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 17:54:57 +0200 Subject: [PATCH 124/316] new start --- TCPServer.cpp | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e8e1d61..cfc68de 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1070,16 +1070,6 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) { whiteDropPosition = std::array{762, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { - if (!potJardiniere2Removed) { - this->go(200, 400); - awaitRobotIdle(); - this->go(200, 900); - awaitRobotIdle(); - this->go(220, 650); - awaitRobotIdle(); - potJardiniere2Removed = true; - } - whiteDropSetup = std::array{300, 612}; whiteDropPosition = std::array{0, 612}; angle = -PI; @@ -1090,16 +1080,6 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) { whiteDropPosition = std::array{2237, 0}; angle = PI / 2; } else if (sp == DROP_WHITE_FLOWER_J2) { - if (!potJardiniere2Removed) { - this->go(2800, 400); - awaitRobotIdle(); - this->go(2800, 900); - awaitRobotIdle(); - this->go(1780, 650); - awaitRobotIdle(); - potJardiniere2Removed = true; - } - whiteDropSetup = std::array{1700, 612}; whiteDropPosition = std::array{0, 612}; angle = 0; From f87fc78c74184514407a489bc974fdbe79eff769 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 18:01:48 +0200 Subject: [PATCH 125/316] new start --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index cfc68de..102531b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1367,9 +1367,9 @@ void TCPServer::removePot(StratPattern sp) { if (sp == REMOVE_POT_J2) { this->go(230, 1000); awaitRobotIdle(); - this->go(200, 500); + this->go(200, 400); awaitRobotIdle(); - this->go(220, 650); + this->go(230, 650); awaitRobotIdle(); } } else if (team == YELLOW) { From b4e566327817c614184e86a1b7ff93d58cfc0916 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 18:04:39 +0200 Subject: [PATCH 126/316] new start --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 102531b..814fd6b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -479,7 +479,7 @@ void TCPServer::startGame() { dropFlowers(DROP_FLOWER_J1); break; case DROP_FLOWER_J2: - dropFlowers(DROP_WHITE_FLOWER_J2); + dropFlowers(DROP_FLOWER_J2); break; case TAKE_3_PLANT_TOP: go3Plants(TAKE_3_PLANT_TOP); From 5339de0d6aaeffab911da824e7ec943f6aad23fe Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 18:05:27 +0200 Subject: [PATCH 127/316] speed --- TCPServer.cpp | 4 ++++ TCPServer.h | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 814fd6b..92bcd9f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1331,6 +1331,7 @@ void TCPServer::go3Plants(StratPattern sp) { this->go(500, 700); awaitRobotIdle(); + this->setSpeed(180); this->rotate(0); awaitRobotIdle(); } @@ -1338,12 +1339,15 @@ void TCPServer::go3Plants(StratPattern sp) { this->go(500, 1300); awaitRobotIdle(); + this->setSpeed(180); this->rotate(0); awaitRobotIdle(); } else { return; } + this->setSpeed(200); + for (int i = 0; i < 3; i++) { this->openPince(i); } diff --git a/TCPServer.h b/TCPServer.h index 6cacfb9..874693e 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -53,7 +53,7 @@ enum StratPattern { DROP_FLOWER_J1, DROP_FLOWER_J2, REMOVE_POT_J2, - DROP_FLOWER_BASE_1, + // DROP_FLOWER_BASE_1, }; class TCPServer; // Forward declaration From 94e7253c9c8f7d7803d877450d4d77fa4a15b890 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 18:07:37 +0200 Subject: [PATCH 128/316] do not need TODOs --- TCPServer.cpp | 4 ++-- TCPServer.h | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 92bcd9f..3ba0d17 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1380,9 +1380,9 @@ void TCPServer::removePot(StratPattern sp) { if (sp == REMOVE_POT_J2) { this->go(2770, 1000); awaitRobotIdle(); - this->go(2800, 500); + this->go(2800, 400); awaitRobotIdle(); - this->go(2780, 650); + this->go(2770, 650); awaitRobotIdle(); } } diff --git a/TCPServer.h b/TCPServer.h index 874693e..308120d 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -108,7 +108,6 @@ private: Team team; - // TODO drop white flower in other jardienière (if time) std::vector stratPatterns = { TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, From 9a6dcac36a6f54b799d42f88242983023388b2cf Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 18:34:26 +0200 Subject: [PATCH 129/316] new strat --- TCPServer.cpp | 147 ++++++++++++++++++++++++++------------------------ TCPServer.h | 19 +++---- 2 files changed, 86 insertions(+), 80 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3ba0d17..a433d04 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -466,11 +466,8 @@ void TCPServer::startGame() { case GET_LIDAR_POS: getLidarPos(); break; - case CHECKPOINT_BOTTOM_TO_TOP: - checkpoint(CHECKPOINT_BOTTOM_TO_TOP); - break; - case CHECKPOINT_TOP_TO_BOTTOM: - checkpoint(CHECKPOINT_TOP_TO_BOTTOM); + case CHECKPOINT_MIDDLE: + checkpoint(CHECKPOINT_MIDDLE); break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: checkpoint(CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER); @@ -481,15 +478,24 @@ void TCPServer::startGame() { case DROP_FLOWER_J2: dropFlowers(DROP_FLOWER_J2); break; - case TAKE_3_PLANT_TOP: - go3Plants(TAKE_3_PLANT_TOP); + case TAKE_3_PLANT_TOP_1: + go3Plants(TAKE_3_PLANT_TOP_1); break; - case TAKE_3_PLANT_BOTTOM: - go3Plants(TAKE_3_PLANT_BOTTOM); + case TAKE_3_PLANT_BOTTOM_1: + go3Plants(TAKE_3_PLANT_BOTTOM_1); + break; + case TAKE_3_PLANT_TOP_2: + go3Plants(TAKE_3_PLANT_TOP_2); + break; + case TAKE_3_PLANT_BOTTOM_2: + go3Plants(TAKE_3_PLANT_BOTTOM_2); break; case REMOVE_POT_J2: removePot(REMOVE_POT_J2); break; + case DROP_FLOWER_BASE_1: + dropFlowers(DROP_FLOWER_BASE_1); + break; } whereAmI++; } @@ -1220,50 +1226,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->setSpeed(previousSpeed); } -void TCPServer::checkpoint(const StratPattern sp) { - this->setSpeed(200); - if (team == BLUE) { - switch (sp) { - case CHECKPOINT_BOTTOM_TO_TOP: - this->go(500, 1000); - awaitRobotIdle(); - this->go(500, 500); - usleep(500'000); - break; - case CHECKPOINT_TOP_TO_BOTTOM: - this->go(500, 1500); - this->awaitRobotIdle(); - break; - case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: - this->go(500, 1700); - usleep(500'000); - break; - default: - break; - } - } else if (team == YELLOW) { - switch (sp) { - case CHECKPOINT_BOTTOM_TO_TOP: - this->go(2500, 1000); - awaitRobotIdle(); - this->go(2500, 500); - usleep(500'000); - break; - case CHECKPOINT_TOP_TO_BOTTOM: - this->go(2500, 1500); - this->awaitRobotIdle(); - break; - case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: - this->go(2500, 1700); - usleep(500'000); - break; - default: - break; - } - } -} - -void TCPServer::dropFlowers(StratPattern sp) { +void TCPServer::dropFlowers(const StratPattern sp) { std::array whiteDropSetup{}; std::array whiteDropPosition{}; @@ -1295,12 +1258,10 @@ void TCPServer::dropFlowers(StratPattern sp) { this->go(whiteDropSetup); awaitRobotIdle(); - this->rotate(angle); awaitRobotIdle(); this->leverBras(); - usleep(500'000); this->setSpeed(130); @@ -1326,26 +1287,40 @@ void TCPServer::dropFlowers(StratPattern sp) { this->transportBras(); } -void TCPServer::go3Plants(StratPattern sp) { - if (sp == TAKE_3_PLANT_TOP) { - this->go(500, 700); - awaitRobotIdle(); +void TCPServer::go3Plants(const StratPattern sp) { + std::array checkpoint{}; + std::array plantPosition{}; - this->setSpeed(180); - this->rotate(0); - awaitRobotIdle(); + double angle; + if (sp == TAKE_3_PLANT_TOP_1) { + checkpoint = {700, 700}; + plantPosition = {900, 700}; + angle = 0; } - else if (sp == TAKE_3_PLANT_BOTTOM) { - this->go(500, 1300); - awaitRobotIdle(); - - this->setSpeed(180); - this->rotate(0); - awaitRobotIdle(); + else if (sp == TAKE_3_PLANT_TOP_2) { + checkpoint = {700, 700}; + plantPosition = {1100, 700}; + angle = 0; + } + else if (sp == TAKE_3_PLANT_BOTTOM_1) { + checkpoint = {700, 1300}; + plantPosition = {900, 1300}; + angle = 0; + } else if (sp == TAKE_3_PLANT_BOTTOM_2) { + checkpoint = {700, 1300}; + plantPosition = {1100, 1300}; + angle = 0; } else { return; } + this->go(checkpoint); + awaitRobotIdle(); + + this->setSpeed(180); + this->rotate(angle); + awaitRobotIdle(); + this->setSpeed(200); for (int i = 0; i < 3; i++) { @@ -1353,7 +1328,7 @@ void TCPServer::go3Plants(StratPattern sp) { } usleep(200'000); - this->go(900, 1300); + this->go(plantPosition); awaitRobotIdle(); for (int i = 0; i < 3; i++) { @@ -1362,7 +1337,7 @@ void TCPServer::go3Plants(StratPattern sp) { } usleep(200'000); - this->go(800, 1300); + this->go(checkpoint); awaitRobotIdle(); } @@ -1404,6 +1379,36 @@ void TCPServer::getLidarPos() { } +void TCPServer::checkpoint(const StratPattern sp) { + this->setSpeed(200); + if (team == BLUE) { + switch (sp) { + case CHECKPOINT_MIDDLE: + this->go(500, 1500); + this->awaitRobotIdle(); + break; + case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: + this->go(500, 1700); + usleep(500'000); + break; + default: + break; + } + } else if (team == YELLOW) { + switch (sp) { + case CHECKPOINT_MIDDLE: + this->go(2500, 1500); + this->awaitRobotIdle(); + break; + case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: + this->go(2500, 1700); + usleep(500'000); + break; + default: + break; + } + } +} template void TCPServer::go(X x, Y y) { diff --git a/TCPServer.h b/TCPServer.h index 308120d..91f94c8 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -45,15 +45,16 @@ enum StratPattern { DROP_WHITE_FLOWER_J2, GO_END, GET_LIDAR_POS, - CHECKPOINT_BOTTOM_TO_TOP, - CHECKPOINT_TOP_TO_BOTTOM, + CHECKPOINT_MIDDLE, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, - TAKE_3_PLANT_BOTTOM, - TAKE_3_PLANT_TOP, + TAKE_3_PLANT_BOTTOM_1, + TAKE_3_PLANT_BOTTOM_2, + TAKE_3_PLANT_TOP_1, + TAKE_3_PLANT_TOP_2, DROP_FLOWER_J1, DROP_FLOWER_J2, REMOVE_POT_J2, - // DROP_FLOWER_BASE_1, + DROP_FLOWER_BASE_1, }; class TCPServer; // Forward declaration @@ -116,7 +117,7 @@ private: /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - TAKE_3_PLANT_BOTTOM, + TAKE_3_PLANT_BOTTOM_1, REMOVE_POT_J2, DROP_FLOWER_J2, /*DROP_PURPLE_FLOWER, @@ -125,7 +126,7 @@ private: /*TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP,*/ - TAKE_3_PLANT_TOP, + TAKE_3_PLANT_TOP_1, DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2,*/ @@ -136,8 +137,8 @@ private: //TAKE_3_PLANT_TOP, /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - // TAKE_3_PLANT_TOP, - // DROP_FLOWER_BASE_1, + TAKE_3_PLANT_TOP_2, + DROP_FLOWER_BASE_1, GO_END }; From ed3bbcdd7c2fb651e074fd967a45e52a9682adff Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 18:42:06 +0200 Subject: [PATCH 130/316] position --- TCPServer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a433d04..01feebd 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1294,7 +1294,7 @@ void TCPServer::go3Plants(const StratPattern sp) { double angle; if (sp == TAKE_3_PLANT_TOP_1) { checkpoint = {700, 700}; - plantPosition = {900, 700}; + plantPosition = {950, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_TOP_2) { @@ -1304,7 +1304,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } else if (sp == TAKE_3_PLANT_BOTTOM_1) { checkpoint = {700, 1300}; - plantPosition = {900, 1300}; + plantPosition = {950, 1300}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { checkpoint = {700, 1300}; @@ -1330,12 +1330,13 @@ void TCPServer::go3Plants(const StratPattern sp) { this->go(plantPosition); awaitRobotIdle(); + usleep(500'000); for (int i = 0; i < 3; i++) { this->closePince(i); pinceState[i] = FLOWER; } - usleep(200'000); + usleep(1'000'000); this->go(checkpoint); awaitRobotIdle(); From 819ab7459d4bf570e5839b50c77be871968769ee Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 18:56:27 +0200 Subject: [PATCH 131/316] pince drop --- TCPServer.cpp | 36 ++++++++++++++++++++++++++++++------ TCPServer.h | 2 ++ 2 files changed, 32 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 01feebd..17af621 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1268,12 +1268,30 @@ void TCPServer::dropFlowers(const StratPattern sp) { this->go(whiteDropPosition); usleep(2'000'000); - for (int i = 0; i < 3; i++) { + this->fullyOpenPince(0); + this->fullyOpenPince(2); + pinceState[0] = NONE; + pinceState[2] = NONE; + + usleep(1'000'000); + + this->closePince(0); + this->closePince(2); + + usleep(200'000); + + this->fullyOpenPince(1); + pinceState[1] = NONE; + + usleep(1'000'000); + + this->closePince(1); + /*for (int i = 0; i < 3; i++) { this->openPince(i); pinceState[i] = NONE; this->sendPoint(4); } - usleep(1'000'000); + usleep(1'000'000);*/ this->setSpeed(200); @@ -1293,21 +1311,21 @@ void TCPServer::go3Plants(const StratPattern sp) { double angle; if (sp == TAKE_3_PLANT_TOP_1) { - checkpoint = {700, 700}; + checkpoint = {600, 700}; plantPosition = {950, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_TOP_2) { - checkpoint = {700, 700}; + checkpoint = {600, 700}; plantPosition = {1100, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { - checkpoint = {700, 1300}; + checkpoint = {600, 1300}; plantPosition = {950, 1300}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { - checkpoint = {700, 1300}; + checkpoint = {600, 1300}; plantPosition = {1100, 1300}; angle = 0; } else { @@ -1340,6 +1358,8 @@ void TCPServer::go3Plants(const StratPattern sp) { this->go(checkpoint); awaitRobotIdle(); + + this->transportBras(); } void TCPServer::removePot(StratPattern sp) { @@ -1497,6 +1517,10 @@ void TCPServer::openPince(int pince) { this->broadcastMessage("strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n"); } +void TCPServer::fullyOpenPince(int pince) { + this->broadcastMessage("strat;servo_moteur;ouvrir total pince;" + std::to_string(pince) + "\n"); +} + void TCPServer::middlePince(int pince) { this->broadcastMessage("strat;servo_moteur;middle pince;" + std::to_string(pince) + "\n"); } diff --git a/TCPServer.h b/TCPServer.h index 91f94c8..6923cc1 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -266,6 +266,8 @@ public: void openPince(int pince); + void fullyOpenPince(int pince); + void middlePince(int pince); void closePince(int pince); From 0fc57b9d9a4041ae9b601c30bb3fecf2ae6e3b83 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:03:02 +0200 Subject: [PATCH 132/316] sleep --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 17af621..6650444 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1354,7 +1354,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->closePince(i); pinceState[i] = FLOWER; } - usleep(1'000'000); + usleep(500'000); this->go(checkpoint); awaitRobotIdle(); From 47e0d9d245a92d08782b310253df6169eb9240e2 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:10:01 +0200 Subject: [PATCH 133/316] angle --- TCPServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6650444..6a2924f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1273,12 +1273,12 @@ void TCPServer::dropFlowers(const StratPattern sp) { pinceState[0] = NONE; pinceState[2] = NONE; - usleep(1'000'000); + usleep(500'000); this->closePince(0); this->closePince(2); - usleep(200'000); + usleep(100'000); this->fullyOpenPince(1); pinceState[1] = NONE; @@ -1335,7 +1335,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->go(checkpoint); awaitRobotIdle(); - this->setSpeed(180); + this->setSpeed(170); this->rotate(angle); awaitRobotIdle(); From 82b3d2c0807bf4cca70243d5e3a603df16f76b21 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:11:41 +0200 Subject: [PATCH 134/316] sleep --- TCPServer.cpp | 8 +------- TCPServer.h | 4 ++-- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6a2924f..ae8c7c0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1283,15 +1283,9 @@ void TCPServer::dropFlowers(const StratPattern sp) { this->fullyOpenPince(1); pinceState[1] = NONE; - usleep(1'000'000); + usleep(500'000); this->closePince(1); - /*for (int i = 0; i < 3; i++) { - this->openPince(i); - pinceState[i] = NONE; - this->sendPoint(4); - } - usleep(1'000'000);*/ this->setSpeed(200); diff --git a/TCPServer.h b/TCPServer.h index 6923cc1..352d765 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -137,8 +137,8 @@ private: //TAKE_3_PLANT_TOP, /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - TAKE_3_PLANT_TOP_2, - DROP_FLOWER_BASE_1, + // TAKE_3_PLANT_TOP_2, + // DROP_FLOWER_BASE_1, GO_END }; From 9f9c5e59aea6b000ace4048962e426ded21a66f6 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:37:56 +0200 Subject: [PATCH 135/316] drop base --- TCPServer.cpp | 79 +++++++++++++++++++++++++++++++++++++++++++++++---- TCPServer.h | 9 ++++-- 2 files changed, 79 insertions(+), 9 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index ae8c7c0..8b882b7 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -473,10 +473,10 @@ void TCPServer::startGame() { checkpoint(CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER); break; case DROP_FLOWER_J1: - dropFlowers(DROP_FLOWER_J1); + dropJardiniereFlowers(DROP_FLOWER_J1); break; case DROP_FLOWER_J2: - dropFlowers(DROP_FLOWER_J2); + dropJardiniereFlowers(DROP_FLOWER_J2); break; case TAKE_3_PLANT_TOP_1: go3Plants(TAKE_3_PLANT_TOP_1); @@ -494,7 +494,7 @@ void TCPServer::startGame() { removePot(REMOVE_POT_J2); break; case DROP_FLOWER_BASE_1: - dropFlowers(DROP_FLOWER_BASE_1); + dropBaseFlowers(DROP_FLOWER_BASE_1); break; } whereAmI++; @@ -1226,7 +1226,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->setSpeed(previousSpeed); } -void TCPServer::dropFlowers(const StratPattern sp) { +void TCPServer::dropJardiniereFlowers(const StratPattern sp) { std::array whiteDropSetup{}; std::array whiteDropPosition{}; @@ -1299,6 +1299,73 @@ void TCPServer::dropFlowers(const StratPattern sp) { this->transportBras(); } +void TCPServer::dropBaseFlowers(StratPattern sp) { + std::array dropPosition{}; + + if (team == BLUE) { + if (sp == DROP_FLOWER_BASE_1) { + dropPosition = {225, 400}; + } + } + else if (team == YELLOW) { + if (sp == DROP_FLOWER_BASE_1) { + dropPosition = {2775, 400}; + } + } + + this->setSpeed(200); + + this->go(dropPosition); + awaitRobotIdle(); + + this->setSpeed(150); + + this->rotate(PI / 2); + awaitRobotIdle(); + + this->baisserBras(); + + this->fullyOpenPince(0); + this->fullyOpenPince(2); + + usleep(200'000); + + this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150); + awaitRobotIdle(); + + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); + awaitRobotIdle(); + + pinceState[0] = NONE; + pinceState[2] = NONE; + this->closePince(0); + this->closePince(2); + usleep(200'000); + + this->sendPoint(3); + this->sendPoint(3); + + this->fullyOpenPince(1); + + usleep(200'000); + + this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150); + awaitRobotIdle(); + + this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); + awaitRobotIdle(); + + pinceState[1] = NONE; + this->closePince(1); + usleep(200'000); + + this->sendPoint(3); + + this->transportBras(); + + this->setSpeed(200); +} + void TCPServer::go3Plants(const StratPattern sp) { std::array checkpoint{}; std::array plantPosition{}; @@ -1306,7 +1373,7 @@ void TCPServer::go3Plants(const StratPattern sp) { double angle; if (sp == TAKE_3_PLANT_TOP_1) { checkpoint = {600, 700}; - plantPosition = {950, 700}; + plantPosition = {900, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_TOP_2) { @@ -1316,7 +1383,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } else if (sp == TAKE_3_PLANT_BOTTOM_1) { checkpoint = {600, 1300}; - plantPosition = {950, 1300}; + plantPosition = {900, 1300}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { checkpoint = {600, 1300}; diff --git a/TCPServer.h b/TCPServer.h index 352d765..48b6789 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -137,8 +137,10 @@ private: //TAKE_3_PLANT_TOP, /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - // TAKE_3_PLANT_TOP_2, - // DROP_FLOWER_BASE_1, + TAKE_3_PLANT_TOP_2, + DROP_FLOWER_BASE_1, + TAKE_3_PLANT_BOTTOM_2, + DROP_FLOWER_BASE_1, GO_END }; @@ -219,7 +221,8 @@ public: void checkpoint(StratPattern sp); - void dropFlowers(StratPattern sp); + void dropJardiniereFlowers(StratPattern sp); + void dropBaseFlowers(StratPattern sp); void go3Plants(StratPattern sp); From a3dacd40782024525d5316a90a1467b123434191 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:42:07 +0200 Subject: [PATCH 136/316] position --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8b882b7..e630d51 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1373,7 +1373,7 @@ void TCPServer::go3Plants(const StratPattern sp) { double angle; if (sp == TAKE_3_PLANT_TOP_1) { checkpoint = {600, 700}; - plantPosition = {900, 700}; + plantPosition = {950, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_TOP_2) { @@ -1383,7 +1383,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } else if (sp == TAKE_3_PLANT_BOTTOM_1) { checkpoint = {600, 1300}; - plantPosition = {900, 1300}; + plantPosition = {950, 1300}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { checkpoint = {600, 1300}; From 187a99d0775dc0c1563204cb3223dc9d9740d634 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:51:35 +0200 Subject: [PATCH 137/316] position --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e630d51..ec07076 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1304,12 +1304,12 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { if (team == BLUE) { if (sp == DROP_FLOWER_BASE_1) { - dropPosition = {225, 400}; + dropPosition = {300, 400}; } } else if (team == YELLOW) { if (sp == DROP_FLOWER_BASE_1) { - dropPosition = {2775, 400}; + dropPosition = {2700, 400}; } } From a44c98acf1bb48238b2f00b057f447f3af6a11f2 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:53:28 +0200 Subject: [PATCH 138/316] strat --- TCPServer.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 48b6789..1748334 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -137,10 +137,10 @@ private: //TAKE_3_PLANT_TOP, /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - TAKE_3_PLANT_TOP_2, - DROP_FLOWER_BASE_1, - TAKE_3_PLANT_BOTTOM_2, - DROP_FLOWER_BASE_1, + // TAKE_3_PLANT_TOP_2, + // DROP_FLOWER_BASE_1, + // TAKE_3_PLANT_BOTTOM_2, + // DROP_FLOWER_BASE_1, GO_END }; From 0b6a93a9fbefcb59b45e3114f6b151ed7cd1803b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:58:16 +0200 Subject: [PATCH 139/316] pince --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index ec07076..bcedb2a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1285,7 +1285,9 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { usleep(500'000); - this->closePince(1); + this->openPince(1); + this->openPince(0); + this->openPince(2); this->setSpeed(200); From a38d53353add5146918320fff25d8cb233c180ad Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 19:58:42 +0200 Subject: [PATCH 140/316] pince --- TCPServer.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index bcedb2a..0322673 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1294,10 +1294,6 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->go(whiteDropSetup); awaitRobotIdle(); - for (int i = 0; i < 3; i++) { - this->closePince(i); - } - this->transportBras(); } From f4da5478b65fa5124da43b07a8dcc5779d48d917 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:05:46 +0200 Subject: [PATCH 141/316] position --- TCPServer.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0322673..9be8cee 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1157,7 +1157,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: - this->go(475, 1790); + this->go(470, 1790); awaitRobotIdle(); this->rotate(0); @@ -1168,7 +1168,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: - this->go(700, 1790); + this->go(675, 1790); awaitRobotIdle(); this->rotate(0); @@ -1295,6 +1295,10 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { awaitRobotIdle(); this->transportBras(); + + this->sendPoint(4); + this->sendPoint(4); + this->sendPoint(4); } void TCPServer::dropBaseFlowers(StratPattern sp) { From ad0dea302b0f0bd8480d1e030511c302c23cd93c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:07:22 +0200 Subject: [PATCH 142/316] strat --- TCPServer.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 1748334..48b6789 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -137,10 +137,10 @@ private: //TAKE_3_PLANT_TOP, /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - // TAKE_3_PLANT_TOP_2, - // DROP_FLOWER_BASE_1, - // TAKE_3_PLANT_BOTTOM_2, - // DROP_FLOWER_BASE_1, + TAKE_3_PLANT_TOP_2, + DROP_FLOWER_BASE_1, + TAKE_3_PLANT_BOTTOM_2, + DROP_FLOWER_BASE_1, GO_END }; From d0e17df65269cde18bcb694ec29b240ce4a55972 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:17:48 +0200 Subject: [PATCH 143/316] strat --- TCPServer.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9be8cee..2c4332e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1153,7 +1153,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(150'000); + usleep(300'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: @@ -1164,7 +1164,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(150'000); + usleep(300'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: @@ -1175,7 +1175,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(150'000); + usleep(300'000); this->uncheckPanneau(7); break; default: @@ -1191,29 +1191,29 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(150'000); + usleep(300'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: - this->go(2525, 1790); + this->go(2530, 1790); awaitRobotIdle(); this->rotate(PI); awaitRobotIdle(); this->checkPanneau(6); - usleep(150'000); + usleep(300'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: - this->go(2300, 1790); + this->go(2325, 1790); awaitRobotIdle(); this->rotate(PI); awaitRobotIdle(); this->checkPanneau(6); - usleep(150'000); + usleep(300'000); this->uncheckPanneau(6); break; default: @@ -1344,9 +1344,6 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->closePince(2); usleep(200'000); - this->sendPoint(3); - this->sendPoint(3); - this->fullyOpenPince(1); usleep(200'000); From f35801facb39ca0cb4d53a85e3e21368b0842efb Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:23:35 +0200 Subject: [PATCH 144/316] strat --- TCPServer.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2c4332e..5c5751e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1366,33 +1366,28 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { } void TCPServer::go3Plants(const StratPattern sp) { - std::array checkpoint{}; std::array plantPosition{}; double angle; if (sp == TAKE_3_PLANT_TOP_1) { - checkpoint = {600, 700}; - plantPosition = {950, 700}; + plantPosition = {1000, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_TOP_2) { - checkpoint = {600, 700}; - plantPosition = {1100, 700}; + plantPosition = {1200, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { - checkpoint = {600, 1300}; - plantPosition = {950, 1300}; + plantPosition = {1000, 1300}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { - checkpoint = {600, 1300}; - plantPosition = {1100, 1300}; + plantPosition = {1200, 1300}; angle = 0; } else { return; } - this->go(checkpoint); + this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); this->setSpeed(170); @@ -1406,7 +1401,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(200'000); - this->go(plantPosition); + this->go(plantPosition[0], this->robotPose.pos.y); awaitRobotIdle(); usleep(500'000); @@ -1416,7 +1411,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(500'000); - this->go(checkpoint); + this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); this->transportBras(); From 8a7a967cccbfd4f7910948461a75de53349df363 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:28:34 +0200 Subject: [PATCH 145/316] strat --- TCPServer.cpp | 37 ++++++++++++++++++++++++++++++++----- TCPServer.h | 3 ++- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5c5751e..7eb547d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -496,6 +496,9 @@ void TCPServer::startGame() { case DROP_FLOWER_BASE_1: dropBaseFlowers(DROP_FLOWER_BASE_1); break; + case DROP_FLOWER_BASE_2: + dropBaseFlowers(DROP_FLOWER_BASE_2); + break; } whereAmI++; } @@ -1303,16 +1306,40 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { void TCPServer::dropBaseFlowers(StratPattern sp) { std::array dropPosition{}; + double angle; + float distance; if (team == BLUE) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {300, 400}; + angle = PI / 2; + distance = 150; + } + else if (sp == DROP_FLOWER_BASE_2) { + dropPosition = {300, 1600}; + angle = -PI / 2; + distance = -150; + } + else { + return; } } else if (team == YELLOW) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {2700, 400}; + angle = PI / 2; + distance = 150; } + else if (sp == DROP_FLOWER_BASE_2) { + dropPosition = {2700, 1600}; + angle = -PI / 2; + distance = -150; + } else { + return; + } + } + else { + return; } this->setSpeed(200); @@ -1322,7 +1349,7 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->setSpeed(150); - this->rotate(PI / 2); + this->rotate(angle); awaitRobotIdle(); this->baisserBras(); @@ -1332,10 +1359,10 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { usleep(200'000); - this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150); + this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance); awaitRobotIdle(); - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); + this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance); awaitRobotIdle(); pinceState[0] = NONE; @@ -1348,10 +1375,10 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { usleep(200'000); - this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150); + this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance); awaitRobotIdle(); - this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); + this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance); awaitRobotIdle(); pinceState[1] = NONE; diff --git a/TCPServer.h b/TCPServer.h index 48b6789..05ffa7c 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -55,6 +55,7 @@ enum StratPattern { DROP_FLOWER_J2, REMOVE_POT_J2, DROP_FLOWER_BASE_1, + DROP_FLOWER_BASE_2, }; class TCPServer; // Forward declaration @@ -140,7 +141,7 @@ private: TAKE_3_PLANT_TOP_2, DROP_FLOWER_BASE_1, TAKE_3_PLANT_BOTTOM_2, - DROP_FLOWER_BASE_1, + DROP_FLOWER_BASE_2, GO_END }; From 3bc4bd0895af4977c68312721886e3d5d349b451 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:46:42 +0200 Subject: [PATCH 146/316] position --- TCPServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7eb547d..b37d99b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1401,14 +1401,14 @@ void TCPServer::go3Plants(const StratPattern sp) { angle = 0; } else if (sp == TAKE_3_PLANT_TOP_2) { - plantPosition = {1200, 700}; + plantPosition = {1300, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { plantPosition = {1000, 1300}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { - plantPosition = {1200, 1300}; + plantPosition = {1300, 1300}; angle = 0; } else { return; @@ -1417,7 +1417,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); - this->setSpeed(170); + this->setSpeed(150); this->rotate(angle); awaitRobotIdle(); From a3221abdbdcfe3f75dc330fb758f98a9c4e664e4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:57:21 +0200 Subject: [PATCH 147/316] reposition --- TCPServer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index b37d99b..4649f17 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1271,6 +1271,14 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->go(whiteDropPosition); usleep(2'000'000); + // reposition + if (sp == DROP_FLOWER_J1) { + this->setPosition(this->robotPose.pos.x, 140, -PI); + } + else if (sp == DROP_FLOWER_J2) { + this->setPosition(140, this->robotPose.pos.y, 0); + } + this->fullyOpenPince(0); this->fullyOpenPince(2); pinceState[0] = NONE; From aae3dd8b2b6ad8d302c274efdaa30f9f581f6e79 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:57:29 +0200 Subject: [PATCH 148/316] reposition --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4649f17..983cc14 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1276,7 +1276,7 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->setPosition(this->robotPose.pos.x, 140, -PI); } else if (sp == DROP_FLOWER_J2) { - this->setPosition(140, this->robotPose.pos.y, 0); + this->setPosition(140, this->robotPose.pos.y, PI / 2); } this->fullyOpenPince(0); From 339d96c907d28797d493ac25a827bd8080f05a20 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 20:58:07 +0200 Subject: [PATCH 149/316] reposition --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 983cc14..58d73ad 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1273,10 +1273,10 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { // reposition if (sp == DROP_FLOWER_J1) { - this->setPosition(this->robotPose.pos.x, 140, -PI); + this->setPosition(this->robotPose.pos.x, 140, angle); } else if (sp == DROP_FLOWER_J2) { - this->setPosition(140, this->robotPose.pos.y, PI / 2); + this->setPosition(140, this->robotPose.pos.y, angle); } this->fullyOpenPince(0); From e2531a466dacf4f4c54190cc01ac3335fbeaa870 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 21:15:25 +0200 Subject: [PATCH 150/316] speed --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 58d73ad..496136c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1355,7 +1355,7 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->go(dropPosition); awaitRobotIdle(); - this->setSpeed(150); + this->setSpeed(170); this->rotate(angle); awaitRobotIdle(); @@ -1425,7 +1425,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); - this->setSpeed(150); + this->setSpeed(170); this->rotate(angle); awaitRobotIdle(); From ccf5e0e91b202183a0fd7e60575a0bc8c75b1a39 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 3 May 2024 21:41:12 +0200 Subject: [PATCH 151/316] include lidar --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 496136c..94cdc5f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -129,6 +129,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->gameThread.~thread(); this->broadcastMessage("strat;arduino;clear;1"); + this->broadcastMessage(message); + std::vector args = TCPUtils::split(tokens[3], ","); if (!handleEmergencyFlag) { @@ -1457,7 +1459,7 @@ void TCPServer::removePot(StratPattern sp) { if (sp == REMOVE_POT_J2) { this->go(230, 1000); awaitRobotIdle(); - this->go(200, 400); + this->go(210, 400); awaitRobotIdle(); this->go(230, 650); awaitRobotIdle(); From c534262794e812b3eeadd20bc83cc60550c92452 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 08:34:38 +0200 Subject: [PATCH 152/316] plant position --- TCPServer.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 94cdc5f..dcb0e0c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1002,8 +1002,7 @@ void TCPServer::dropPurpleFlowers() { case PURPLE_FLOWER: pinceHavePurpleFlower.push_back(i); break; - case WHITE_FLOWER: - case NONE: + default: break; } } @@ -1388,7 +1387,7 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance); awaitRobotIdle(); - this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance); + this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance*5); awaitRobotIdle(); pinceState[1] = NONE; @@ -1407,18 +1406,18 @@ void TCPServer::go3Plants(const StratPattern sp) { double angle; if (sp == TAKE_3_PLANT_TOP_1) { - plantPosition = {1000, 700}; + plantPosition = {950, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_TOP_2) { - plantPosition = {1300, 700}; + plantPosition = {1200, 700}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { - plantPosition = {1000, 1300}; + plantPosition = {950, 1300}; angle = 0; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { - plantPosition = {1300, 1300}; + plantPosition = {1200, 1300}; angle = 0; } else { return; From 19e5d768d704d84acbea4b5178248926cf697f97 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 08:42:49 +0200 Subject: [PATCH 153/316] reposition --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index dcb0e0c..2ecc0ce 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1274,10 +1274,10 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { // reposition if (sp == DROP_FLOWER_J1) { - this->setPosition(this->robotPose.pos.x, 140, angle); + this->setPosition(this->robotPose.pos.x, 134, angle); } else if (sp == DROP_FLOWER_J2) { - this->setPosition(140, this->robotPose.pos.y, angle); + this->setPosition(134, this->robotPose.pos.y, angle); } this->fullyOpenPince(0); From 68daf82f6a3141e5a4d43213117c4c0e473a01fd Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 08:56:54 +0200 Subject: [PATCH 154/316] reposition --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2ecc0ce..740249a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1274,10 +1274,10 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { // reposition if (sp == DROP_FLOWER_J1) { - this->setPosition(this->robotPose.pos.x, 134, angle); + this->setPosition(this->robotPose.pos.x, 142, this->robotPose.theta); } else if (sp == DROP_FLOWER_J2) { - this->setPosition(134, this->robotPose.pos.y, angle); + this->setPosition(142, this->robotPose.pos.y, this->robotPose.theta); } this->fullyOpenPince(0); From 32ce4e2892115c523b28e13a0bc7531b057bc29f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 08:57:34 +0200 Subject: [PATCH 155/316] reposition --- TCPServer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 740249a..19a116d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1277,7 +1277,12 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->setPosition(this->robotPose.pos.x, 142, this->robotPose.theta); } else if (sp == DROP_FLOWER_J2) { - this->setPosition(142, this->robotPose.pos.y, this->robotPose.theta); + if (team == BLUE) { + this->setPosition(142, this->robotPose.pos.y, this->robotPose.theta); + } + else if (team == YELLOW) { + this->setPosition(2858, this->robotPose.pos.y, this->robotPose.theta); + } } this->fullyOpenPince(0); From efef36e487c49c8095839e45b7cc195793307522 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:06:59 +0200 Subject: [PATCH 156/316] take flower --- TCPServer.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 19a116d..6e98069 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1408,22 +1408,27 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { void TCPServer::go3Plants(const StratPattern sp) { std::array plantPosition{}; + double direction; double angle; if (sp == TAKE_3_PLANT_TOP_1) { plantPosition = {950, 700}; angle = 0; + direction = 1; } else if (sp == TAKE_3_PLANT_TOP_2) { plantPosition = {1200, 700}; angle = 0; + direction = 1; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { plantPosition = {950, 1300}; angle = 0; + direction = -1; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { plantPosition = {1200, 1300}; angle = 0; + direction = -1; } else { return; } @@ -1452,9 +1457,22 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(500'000); - this->go(plantPosition[0]-400, plantPosition[1]); + this->go(plantPosition[0]-700, plantPosition[1]); awaitRobotIdle(); + for (int i = 0; i < 3; i++) { + this->openPince(i); + } + usleep(200'000); + + this->go(this->robotPose.pos.x + (200 * direction), this->robotPose.pos.y); + awaitRobotIdle(); + + for (int i = 0; i < 3; i++) { + this->closePince(i); + } + usleep(500'000); + this->transportBras(); } From 5108e366be4cf5fc420ec4c08b646c4115d6a616 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:09:36 +0200 Subject: [PATCH 157/316] take flower --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6e98069..da80ec2 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1457,7 +1457,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(500'000); - this->go(plantPosition[0]-700, plantPosition[1]); + this->go(plantPosition[0]-300, plantPosition[1]); awaitRobotIdle(); for (int i = 0; i < 3; i++) { From 1cdb53e599f8a290ab60cc2845645be5369586dc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:13:43 +0200 Subject: [PATCH 158/316] wait more --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index da80ec2..21eb381 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -758,6 +758,7 @@ void TCPServer::awaitRobotIdle() { int timeout = 0; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop + usleep(100'000); while (isRobotIdle < 3) { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); From 7b146339f2aa5c49dd5b43993a5d444c4c25fb68 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:14:17 +0200 Subject: [PATCH 159/316] wait more --- TCPServer.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 05ffa7c..2fb110c 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -119,8 +119,7 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ TAKE_3_PLANT_BOTTOM_1, - REMOVE_POT_J2, - DROP_FLOWER_J2, + DROP_FLOWER_BASE_2, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ // GET_LIDAR_POS, @@ -141,7 +140,8 @@ private: TAKE_3_PLANT_TOP_2, DROP_FLOWER_BASE_1, TAKE_3_PLANT_BOTTOM_2, - DROP_FLOWER_BASE_2, + REMOVE_POT_J2, + DROP_FLOWER_J2, GO_END }; From 50a0710772b6a8d8d3c274f0787254bf0db0cf73 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:15:04 +0200 Subject: [PATCH 160/316] strat --- TCPServer.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 2fb110c..5284c19 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -120,6 +120,9 @@ private: TAKE_FLOWER_BOTTOM,*/ TAKE_3_PLANT_BOTTOM_1, DROP_FLOWER_BASE_2, + TAKE_3_PLANT_BOTTOM_2, + REMOVE_POT_J2, + DROP_FLOWER_J2, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ // GET_LIDAR_POS, @@ -139,9 +142,6 @@ private: DROP_PURPLE_FLOWER,*/ TAKE_3_PLANT_TOP_2, DROP_FLOWER_BASE_1, - TAKE_3_PLANT_BOTTOM_2, - REMOVE_POT_J2, - DROP_FLOWER_J2, GO_END }; From 86434028aa37bd6cd80dd6dec991ef575dcc40ef Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:21:10 +0200 Subject: [PATCH 161/316] depot plante base --- TCPServer.cpp | 39 +++++++++++++-------------------------- TCPServer.h | 1 + 2 files changed, 14 insertions(+), 26 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 21eb381..4d65ff0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1369,10 +1369,9 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->baisserBras(); - this->fullyOpenPince(0); - this->fullyOpenPince(2); - - usleep(200'000); + for (int i = 0; i < 3; i++) { + this->openPince(i); + } this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance); awaitRobotIdle(); @@ -1380,25 +1379,10 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance); awaitRobotIdle(); - pinceState[0] = NONE; - pinceState[2] = NONE; - this->closePince(0); - this->closePince(2); - usleep(200'000); - - this->fullyOpenPince(1); - - usleep(200'000); - - this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance); - awaitRobotIdle(); - - this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance*5); - awaitRobotIdle(); - - pinceState[1] = NONE; - this->closePince(1); - usleep(200'000); + for (int i = 0; i < 3; i++) { + pinceState[i] = NONE; + this->closePince(i); + } this->sendPoint(3); @@ -1425,11 +1409,11 @@ void TCPServer::go3Plants(const StratPattern sp) { else if (sp == TAKE_3_PLANT_BOTTOM_1) { plantPosition = {950, 1300}; angle = 0; - direction = -1; + direction = 1; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { plantPosition = {1200, 1300}; angle = 0; - direction = -1; + direction = 1; } else { return; } @@ -1458,7 +1442,10 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(500'000); - this->go(plantPosition[0]-300, plantPosition[1]); + this->rotate(angle); + awaitRobotIdle(); + + this->go(this->robotPose.pos.x-300, this->robotPose.pos.y); awaitRobotIdle(); for (int i = 0; i < 3; i++) { diff --git a/TCPServer.h b/TCPServer.h index 5284c19..1f874bd 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -223,6 +223,7 @@ public: void checkpoint(StratPattern sp); void dropJardiniereFlowers(StratPattern sp); + void dropBaseFlowers(StratPattern sp); void go3Plants(StratPattern sp); From c66e15596c060af7c28ccb1131988d039417e65d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:22:03 +0200 Subject: [PATCH 162/316] depot plante base --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4d65ff0..87381ea 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -188,7 +188,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) case 3: this->team = BLUE; spawnPoint[0] = 250; - spawnPoint[1] = 1790; + spawnPoint[1] = 1800; spawnPoint[2] = 0; // For test From ac4c5f51cb78efe098acd6a1916cc073d0745309 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:30:37 +0200 Subject: [PATCH 163/316] spawnpoint --- TCPServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 87381ea..f41e7cc 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1151,7 +1151,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: - this->go(250, 1790); + this->go(250, 1800); awaitRobotIdle(); this->rotate(0); @@ -1162,7 +1162,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: - this->go(470, 1790); + this->go(470, 1800); awaitRobotIdle(); this->rotate(0); @@ -1173,7 +1173,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: - this->go(675, 1790); + this->go(675, 1800); awaitRobotIdle(); this->rotate(0); From 6d070eec875f84fd1a5c96455ad02d8578bbaa0d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:31:07 +0200 Subject: [PATCH 164/316] test solar panel --- TCPServer.h | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 1f874bd..6ec3352 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -114,23 +114,28 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, - CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + // CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - TAKE_3_PLANT_BOTTOM_1, - DROP_FLOWER_BASE_2, - TAKE_3_PLANT_BOTTOM_2, - REMOVE_POT_J2, - DROP_FLOWER_J2, + + // TAKE_3_PLANT_BOTTOM_1, + // DROP_FLOWER_BASE_2, + // TAKE_3_PLANT_BOTTOM_2, + // REMOVE_POT_J2, + // DROP_FLOWER_J2, + /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ // GET_LIDAR_POS, /*TAKE_FLOWER_TOP, TAKE_FLOWER_TOP, TAKE_FLOWER_TOP,*/ - TAKE_3_PLANT_TOP_1, - DROP_FLOWER_J1, + + // TAKE_3_PLANT_TOP_1, + // DROP_FLOWER_J1, + /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2,*/ // GET_LIDAR_POS, @@ -140,8 +145,10 @@ private: //TAKE_3_PLANT_TOP, /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - TAKE_3_PLANT_TOP_2, - DROP_FLOWER_BASE_1, + + // TAKE_3_PLANT_TOP_2, + // DROP_FLOWER_BASE_1, + GO_END }; From 9a0246db0cb04e54e319adf7209ed03ef16b2da7 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:43:18 +0200 Subject: [PATCH 165/316] solar panel position --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f41e7cc..bafd017 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1173,7 +1173,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: - this->go(675, 1800); + this->go(650, 1800); awaitRobotIdle(); this->rotate(0); From d62ee9c04376bd676a1ad64dc1938f1d33972a0b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:45:43 +0200 Subject: [PATCH 166/316] solar panel position --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index bafd017..4ccd096 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1162,7 +1162,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: - this->go(470, 1800); + this->go(465, 1800); awaitRobotIdle(); this->rotate(0); From 5cb0b3bd081248a7cec5f7b405dbf1ff00e016ca Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 09:46:13 +0200 Subject: [PATCH 167/316] full strat --- TCPServer.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 6ec3352..66291d5 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -114,17 +114,17 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, - // CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - // TAKE_3_PLANT_BOTTOM_1, - // DROP_FLOWER_BASE_2, - // TAKE_3_PLANT_BOTTOM_2, - // REMOVE_POT_J2, - // DROP_FLOWER_J2, + TAKE_3_PLANT_BOTTOM_1, + DROP_FLOWER_BASE_2, + TAKE_3_PLANT_BOTTOM_2, + REMOVE_POT_J2, + DROP_FLOWER_J2, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ @@ -133,8 +133,8 @@ private: TAKE_FLOWER_TOP, TAKE_FLOWER_TOP,*/ - // TAKE_3_PLANT_TOP_1, - // DROP_FLOWER_J1, + TAKE_3_PLANT_TOP_1, + DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2,*/ @@ -146,8 +146,8 @@ private: /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - // TAKE_3_PLANT_TOP_2, - // DROP_FLOWER_BASE_1, + TAKE_3_PLANT_TOP_2, + DROP_FLOWER_BASE_1, GO_END }; From 04bf1c154d40ca2ee47a6480b7d0d3b38ab872da Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 10:19:36 +0200 Subject: [PATCH 168/316] speed --- TCPServer.cpp | 36 +++++++++++++++++++++++++----------- TCPServer.h | 7 +++++++ utils.h | 2 +- 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4ccd096..157a07f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -203,7 +203,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) case 6: this->team = YELLOW; spawnPoint[0] = 1750; - spawnPoint[1] = 1790; + spawnPoint[1] = 1800; spawnPoint[2] = PI; finishPoint[0] = 2600; @@ -215,11 +215,11 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->team = TEST; spawnPoint[0] = 1200; spawnPoint[1] = 1800; - spawnPoint[2] = 1.57; + spawnPoint[2] = PI / 2; finishPoint[0] = 1200; finishPoint[1] = 1800; - finishPoint[2] = 1.57; + finishPoint[2] = PI / 2; break; } @@ -1147,7 +1147,7 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) { void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { int previousSpeed = this->speed; - this->setSpeed(200); + this->setMaxSpeed(); if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: @@ -1162,7 +1162,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: - this->go(465, 1800); + this->go(455, 1800); awaitRobotIdle(); this->rotate(0); @@ -1258,7 +1258,7 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { } } - this->setSpeed(200); + this->setMaxSpeed(); this->go(whiteDropSetup); awaitRobotIdle(); @@ -1307,7 +1307,7 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->openPince(0); this->openPince(2); - this->setSpeed(200); + this->setMaxSpeed(); this->go(whiteDropSetup); awaitRobotIdle(); @@ -1357,7 +1357,7 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { return; } - this->setSpeed(200); + this->setMaxSpeed(); this->go(dropPosition); awaitRobotIdle(); @@ -1367,6 +1367,8 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->rotate(angle); awaitRobotIdle(); + this->setMaxSpeed(); + this->baisserBras(); for (int i = 0; i < 3; i++) { @@ -1388,7 +1390,7 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->transportBras(); - this->setSpeed(200); + this->setMaxSpeed(); } void TCPServer::go3Plants(const StratPattern sp) { @@ -1425,7 +1427,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->rotate(angle); awaitRobotIdle(); - this->setSpeed(200); + this->setMaxSpeed(); for (int i = 0; i < 3; i++) { this->openPince(i); @@ -1442,9 +1444,12 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(500'000); + this->setSpeed(170); this->rotate(angle); awaitRobotIdle(); + this->setMaxSpeed(); + this->go(this->robotPose.pos.x-300, this->robotPose.pos.y); awaitRobotIdle(); @@ -1465,6 +1470,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } void TCPServer::removePot(StratPattern sp) { + this->setSpeed(200); if (team == BLUE) { if (sp == REMOVE_POT_J2) { this->go(230, 1000); @@ -1503,7 +1509,7 @@ void TCPServer::getLidarPos() { } void TCPServer::checkpoint(const StratPattern sp) { - this->setSpeed(200); + this->setMaxSpeed(); if (team == BLUE) { switch (sp) { case CHECKPOINT_MIDDLE: @@ -1553,6 +1559,14 @@ void TCPServer::setSpeed(const int speed) { this->speed = speed; } +void TCPServer::setMaxSpeed() { + this->setSpeed(MAX_SPEED); +} + +void TCPServer::setMinSpeed() { + this->setSpeed(MIN_SPEED); +} + template void TCPServer::transit(X x, Y y, const int endSpeed) { this->broadcastMessage("strat;arduino;transit;" + std::to_string(static_cast(x)) + "," + std::to_string(static_cast(y)) + "," + std::to_string(endSpeed) + "\n"); diff --git a/TCPServer.h b/TCPServer.h index 66291d5..ac66b40 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -17,6 +17,9 @@ #include "utils.h" +#define MAX_SPEED 250 +#define MIN_SPEED 150 + struct ClientTCP { std::string name; @@ -245,6 +248,10 @@ public: // Call to broadcast void setSpeed(int speed); + void setMaxSpeed(); + + void setMinSpeed(); + template void go(X x, Y y); diff --git a/utils.h b/utils.h index ce106bc..a311cfb 100644 --- a/utils.h +++ b/utils.h @@ -7,7 +7,7 @@ #include #include -#define PI 3.14159265358979323846 +#define PI M_PI enum PinceState { WHITE_FLOWER, From 22827a1923993e655c3be1f2bd9050c54c549550 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 10:27:13 +0200 Subject: [PATCH 169/316] speed --- TCPServer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.h b/TCPServer.h index ac66b40..be3e301 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -17,7 +17,7 @@ #include "utils.h" -#define MAX_SPEED 250 +#define MAX_SPEED 200 #define MIN_SPEED 150 struct ClientTCP From 87f5c6bae568948137de4e339e8f1cb257b6bb95 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 10:41:31 +0200 Subject: [PATCH 170/316] lidar --- TCPServer.cpp | 22 +++++++++++++--------- TCPServer.h | 6 ++++-- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 157a07f..4eab726 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -227,6 +227,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) file << finishPoint[0] << " " << finishPoint[1]; file.close(); + this->sendToClient("strat;lidar;set team;" + std::to_string(this->team) + "\n", lidarSocket); + this->sendToClient("strat;lidar;set beacon;1", lidarSocket); + this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; @@ -768,6 +771,7 @@ void TCPServer::awaitRobotIdle() { break; } } + usleep(50'000); } void TCPServer::handleArucoTag(const ArucoTag &tag) { @@ -1146,8 +1150,7 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) { } void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { - int previousSpeed = this->speed; - this->setMaxSpeed(); + this->setSpeed(170); if (team == BLUE) { switch (sp) { case TURN_SOLAR_PANNEL_1: @@ -1227,8 +1230,6 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { } this->sendPoint(5); - - this->setSpeed(previousSpeed); } void TCPServer::dropJardiniereFlowers(const StratPattern sp) { @@ -1420,10 +1421,12 @@ void TCPServer::go3Plants(const StratPattern sp) { return; } - this->go(plantPosition[0]-400, plantPosition[1]); + this->setMaxSpeed(); + + this->transit(plantPosition[0]-400, plantPosition[1], 150); awaitRobotIdle(); - this->setSpeed(170); + this->setSpeed(150); this->rotate(angle); awaitRobotIdle(); @@ -1434,8 +1437,9 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(200'000); - this->go(plantPosition[0], this->robotPose.pos.y); + this->transit(plantPosition[0], this->robotPose.pos.y, 130); awaitRobotIdle(); + usleep(500'000); for (int i = 0; i < 3; i++) { @@ -1444,7 +1448,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(500'000); - this->setSpeed(170); + this->setSpeed(150); this->rotate(angle); awaitRobotIdle(); @@ -1470,7 +1474,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } void TCPServer::removePot(StratPattern sp) { - this->setSpeed(200); + this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { this->go(230, 1000); diff --git a/TCPServer.h b/TCPServer.h index be3e301..576cd42 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -17,7 +17,7 @@ #include "utils.h" -#define MAX_SPEED 200 +#define MAX_SPEED 170 #define MIN_SPEED 150 struct ClientTCP @@ -32,8 +32,8 @@ struct ClientTCP }; enum Team { - YELLOW, BLUE, + YELLOW, TEST }; @@ -118,6 +118,7 @@ private: TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + GET_LIDAR_POS, /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, @@ -128,6 +129,7 @@ private: TAKE_3_PLANT_BOTTOM_2, REMOVE_POT_J2, DROP_FLOWER_J2, + GET_LIDAR_POS, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ From 5390772b0aed4e39633f43101d6458ed156bab34 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 10:50:47 +0200 Subject: [PATCH 171/316] lidar --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4eab726..802d24d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -227,8 +227,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) file << finishPoint[0] << " " << finishPoint[1]; file.close(); - this->sendToClient("strat;lidar;set team;" + std::to_string(this->team) + "\n", lidarSocket); - this->sendToClient("strat;lidar;set beacon;1", lidarSocket); + this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); + this->broadcastMessage("strat;lidar;set beacon;1"); this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; From 5a033919847cf3afcac894a470b7456f8ef1112d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 10:52:15 +0200 Subject: [PATCH 172/316] strat with lidar --- TCPServer.h | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 576cd42..5de977c 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -114,22 +114,22 @@ private: Team team; std::vector stratPatterns = { - TURN_SOLAR_PANNEL_1, - TURN_SOLAR_PANNEL_2, - TURN_SOLAR_PANNEL_3, - CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + // TURN_SOLAR_PANNEL_1, + // TURN_SOLAR_PANNEL_2, + // TURN_SOLAR_PANNEL_3, + // CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, GET_LIDAR_POS, /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - TAKE_3_PLANT_BOTTOM_1, - DROP_FLOWER_BASE_2, - TAKE_3_PLANT_BOTTOM_2, - REMOVE_POT_J2, - DROP_FLOWER_J2, - GET_LIDAR_POS, + // TAKE_3_PLANT_BOTTOM_1, + // DROP_FLOWER_BASE_2, + // TAKE_3_PLANT_BOTTOM_2, + // REMOVE_POT_J2, + // DROP_FLOWER_J2, + // GET_LIDAR_POS, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ @@ -138,8 +138,8 @@ private: TAKE_FLOWER_TOP, TAKE_FLOWER_TOP,*/ - TAKE_3_PLANT_TOP_1, - DROP_FLOWER_J1, + // TAKE_3_PLANT_TOP_1, + // DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2,*/ @@ -151,8 +151,8 @@ private: /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - TAKE_3_PLANT_TOP_2, - DROP_FLOWER_BASE_1, + // TAKE_3_PLANT_TOP_2, + // DROP_FLOWER_BASE_1, GO_END }; From 99447cd48649445bf0d89baa9519c84254a23911 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 10:55:58 +0200 Subject: [PATCH 173/316] ONLY lidar --- TCPServer.cpp | 1 + TCPServer.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 802d24d..0cd985a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -170,6 +170,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); } else if (tokens[0] == "lidar" && tokens[2] == "set pos") { + this->broadcastMessage(tokens[0] + ";client;" + tokens[2] + ";" + tokens[3] + "\n"); std::vector args = TCPUtils::split(tokens[3], ","); this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), std::stof(args[2]) / 100}; this->setPosition(this->lidarCalculatePos); diff --git a/TCPServer.h b/TCPServer.h index 5de977c..ce5a951 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -154,7 +154,7 @@ private: // TAKE_3_PLANT_TOP_2, // DROP_FLOWER_BASE_1, - GO_END + // GO_END }; // This is the index of the current pattern From e75a371aa23f3d90f4d883af3bdbe5aa35e1fd26 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:01:04 +0200 Subject: [PATCH 174/316] test pos --- TCPServer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0cd985a..3be5799 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -188,8 +188,12 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) switch (spawnPointNb) { case 3: this->team = BLUE; - spawnPoint[0] = 250; + /*spawnPoint[0] = 250; spawnPoint[1] = 1800; + spawnPoint[2] = 0;*/ + + spawnPoint[0] = 500; + spawnPoint[1] = 1000; spawnPoint[2] = 0; // For test From 09347334d920c40b96f7494a2abf6cbfba3bc806 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:07:24 +0200 Subject: [PATCH 175/316] sleep --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3be5799..f32d094 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1503,6 +1503,8 @@ void TCPServer::removePot(StratPattern sp) { void TCPServer::getLidarPos() { + usleep(10'000'000); + this->askLidarPosition(); awaitForLidar = true; From c144e39572893de24c848e8750833acd22796da5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:08:30 +0200 Subject: [PATCH 176/316] team --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f32d094..e988aed 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -232,7 +232,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) file << finishPoint[0] << " " << finishPoint[1]; file.close(); - this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); + this->broadcastMessage("strat;lidar;set team;" + std::to_string((this->team == BLUE ? 0 : 1)) + "\n"); this->broadcastMessage("strat;lidar;set beacon;1"); this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; From 8790d1a4678c1ae8cbbdeabf1086de10e11799fc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:09:00 +0200 Subject: [PATCH 177/316] \n --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e988aed..33d710b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -233,7 +233,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) file.close(); this->broadcastMessage("strat;lidar;set team;" + std::to_string((this->team == BLUE ? 0 : 1)) + "\n"); - this->broadcastMessage("strat;lidar;set beacon;1"); + this->broadcastMessage("strat;lidar;set beacon;1\n"); this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; From 2d239a5fdb18cba4c337dab588346a1b9c03ec16 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:14:11 +0200 Subject: [PATCH 178/316] setPos --- TCPServer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 33d710b..c266230 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -232,9 +232,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) file << finishPoint[0] << " " << finishPoint[1]; file.close(); - this->broadcastMessage("strat;lidar;set team;" + std::to_string((this->team == BLUE ? 0 : 1)) + "\n"); - this->broadcastMessage("strat;lidar;set beacon;1\n"); - this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]}; this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; @@ -243,6 +240,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->setPosition(this->initRobotPose); usleep(200'000); } + + this->broadcastMessage("strat;lidar;set team;" + std::to_string((this->team == BLUE ? 0 : 1)) + "\n"); + this->broadcastMessage("strat;lidar;set beacon;1\n"); } else if (tokens[1] == "strat" && tokens[2] == "start") { @@ -439,11 +439,11 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { - auto time = std::chrono::system_clock::now(); + /*auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(82)) { this->goEnd(); return; - } + }*/ switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: From d495fa2ffa6f80fd5903aa68230d21bf93b691e7 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:24:32 +0200 Subject: [PATCH 179/316] spawn point --- TCPServer.cpp | 10 +++++----- utils.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c266230..60ea5fd 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -188,14 +188,14 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) switch (spawnPointNb) { case 3: this->team = BLUE; - /*spawnPoint[0] = 250; + spawnPoint[0] = 250; spawnPoint[1] = 1800; - spawnPoint[2] = 0;*/ - - spawnPoint[0] = 500; - spawnPoint[1] = 1000; spawnPoint[2] = 0; + /*spawnPoint[0] = 500; + spawnPoint[1] = 1000; + spawnPoint[2] = 0;*/ + // For test /*finishPoint[0] = 400; finishPoint[1] = 1790; diff --git a/utils.h b/utils.h index a311cfb..ce106bc 100644 --- a/utils.h +++ b/utils.h @@ -7,7 +7,7 @@ #include #include -#define PI M_PI +#define PI 3.14159265358979323846 enum PinceState { WHITE_FLOWER, From f1a332405ca987620d3ccbd0c103b865d3759f84 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:28:28 +0200 Subject: [PATCH 180/316] remove emergency --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 60ea5fd..2cd3647 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -125,7 +125,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (tokens[2] == "stop proximity") { if (!gameStarted) return; - this->stopEmergency = true; +/* this->stopEmergency = true; this->gameThread.~thread(); this->broadcastMessage("strat;arduino;clear;1"); @@ -135,7 +135,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (!handleEmergencyFlag) { std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1]) / 100); }).detach(); - } + }*/ } else if (tokens[1] != "strat") { From 085600723257e33302506792f0c5347d959a5275 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:32:15 +0200 Subject: [PATCH 181/316] await for lidar --- TCPServer.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2cd3647..ef951a0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -188,9 +188,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) switch (spawnPointNb) { case 3: this->team = BLUE; - spawnPoint[0] = 250; + /*spawnPoint[0] = 250; spawnPoint[1] = 1800; - spawnPoint[2] = 0; + spawnPoint[2] = 0;*/ /*spawnPoint[0] = 500; spawnPoint[1] = 1000; @@ -756,7 +756,9 @@ void TCPServer::askArduinoPos() { } while (!this->_shouldStop) { - this->sendToClient("strat;arduino;get pos;1\n", this->arduinoSocket); + if (!awaitForLidar) { + this->sendToClient("strat;arduino;get pos;1\n", this->arduinoSocket); + }; usleep(200'000); } } From 3adeaf1e305a8bbd6ecfaece513b0490022f138a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:35:47 +0200 Subject: [PATCH 182/316] spawn --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index ef951a0..631797f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -182,8 +182,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) else if (tokens[0] == "ihm") { if (tokens[2] == "spawn") { int spawnPointNb = std::stoi(tokens[3]); - float spawnPoint[3]; - float finishPoint[3]; + std::array spawnPoint{}; + std::array finishPoint{}; switch (spawnPointNb) { case 3: @@ -192,9 +192,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) spawnPoint[1] = 1800; spawnPoint[2] = 0;*/ - /*spawnPoint[0] = 500; + spawnPoint[0] = 500; spawnPoint[1] = 1000; - spawnPoint[2] = 0;*/ + spawnPoint[2] = 0; // For test /*finishPoint[0] = 400; From d77f5e1739c903cf77097f5363b459512cbabb04 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:37:22 +0200 Subject: [PATCH 183/316] spawn --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 631797f..e723ef2 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -237,6 +237,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; for (int j = 0; j < 3; j++) { + std::cout << "AHHHHHHHHHHHHHHHHHH " << this->initRobotPose.pos.x << " " << this->initRobotPose.pos.y << " " << this->initRobotPose.theta << std::endl; this->setPosition(this->initRobotPose); usleep(200'000); } From cf88cc5d458d68808b505de247dc2be8ab25f50c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 11:37:50 +0200 Subject: [PATCH 184/316] sleep --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e723ef2..d4a38bf 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -239,7 +239,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) for (int j = 0; j < 3; j++) { std::cout << "AHHHHHHHHHHHHHHHHHH " << this->initRobotPose.pos.x << " " << this->initRobotPose.pos.y << " " << this->initRobotPose.theta << std::endl; this->setPosition(this->initRobotPose); - usleep(200'000); + usleep(1'000'000); } this->broadcastMessage("strat;lidar;set team;" + std::to_string((this->team == BLUE ? 0 : 1)) + "\n"); From 586ad09ddebab25c59a51f2862d20146d37c9f7c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 12:07:30 +0200 Subject: [PATCH 185/316] go --- TCPServer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d4a38bf..22db9f5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -237,7 +237,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]}; for (int j = 0; j < 3; j++) { - std::cout << "AHHHHHHHHHHHHHHHHHH " << this->initRobotPose.pos.x << " " << this->initRobotPose.pos.y << " " << this->initRobotPose.theta << std::endl; this->setPosition(this->initRobotPose); usleep(1'000'000); } @@ -255,6 +254,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->setSpeed(200); + this->go(this->initRobotPose[0], this->initRobotPose[1]); + switch (this->team) { case BLUE: case YELLOW: From 820ea5ff962c2415595b2f0867b98a54ee62f81f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 12:07:42 +0200 Subject: [PATCH 186/316] go --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 22db9f5..7b6abe8 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -254,7 +254,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->setSpeed(200); - this->go(this->initRobotPose[0], this->initRobotPose[1]); + this->go(this->initRobotPose.pos.x, this->initRobotPose.pos.y); switch (this->team) { case BLUE: From 38f1f9b8049bca6e83815551f4de9122b4ed6b2b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 4 May 2024 22:06:00 +0200 Subject: [PATCH 187/316] rafactoring --- TCPServer.cpp | 98 +++++++++++++++++++++++++++++++++------------------ TCPServer.h | 32 ++++++++--------- 2 files changed, 79 insertions(+), 51 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7b6abe8..3e157f7 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -125,7 +125,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (tokens[2] == "stop proximity") { if (!gameStarted) return; -/* this->stopEmergency = true; + this->stopEmergency = true; this->gameThread.~thread(); this->broadcastMessage("strat;arduino;clear;1"); @@ -135,19 +135,16 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (!handleEmergencyFlag) { std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1]) / 100); }).detach(); - }*/ + } } - else if (tokens[1] != "strat") - { + else if (tokens[1] != "strat") { this->broadcastMessage(message, clientSocket); } // EMERGENCY - else if (tokens[0] == "tirette" && tokens[2] == "set state") - { + else if (tokens[0] == "tirette" && tokens[2] == "set state") { this->broadcastMessage(message, clientSocket); } - else if (tokens[2] == "ready") - { + else if (tokens[2] == "ready") { for (ClientTCP& client : clients) { if (client.name == tokens[0]) @@ -178,7 +175,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->setPosition(this->lidarCalculatePos); awaitForLidar = false; } - else if (tokens[0] == "ihm") { if (tokens[2] == "spawn") { int spawnPointNb = std::stoi(tokens[3]); @@ -188,14 +184,14 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) switch (spawnPointNb) { case 3: this->team = BLUE; - /*spawnPoint[0] = 250; + spawnPoint[0] = 250; spawnPoint[1] = 1800; - spawnPoint[2] = 0;*/ - - spawnPoint[0] = 500; - spawnPoint[1] = 1000; spawnPoint[2] = 0; + /*spawnPoint[0] = 500; + spawnPoint[1] = 1000; + spawnPoint[2] = 0;*/ + // For test /*finishPoint[0] = 400; finishPoint[1] = 1790; @@ -441,11 +437,11 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { - /*auto time = std::chrono::system_clock::now(); + auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(82)) { this->goEnd(); return; - }*/ + } switch (stratPatterns[i]) { case TURN_SOLAR_PANNEL_1: @@ -1283,7 +1279,7 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->go(whiteDropPosition); usleep(2'000'000); - // reposition + // reposition, TODO remove when lidar is working if (sp == DROP_FLOWER_J1) { this->setPosition(this->robotPose.pos.x, 142, this->robotPose.theta); } @@ -1408,45 +1404,77 @@ void TCPServer::go3Plants(const StratPattern sp) { double direction; double angle; - if (sp == TAKE_3_PLANT_TOP_1) { - plantPosition = {950, 700}; + if (team == BLUE) { angle = 0; direction = 1; + if (sp == TAKE_3_PLANT_TOP_1) { + plantPosition = {950, 700}; + } + else if (sp == TAKE_3_PLANT_TOP_2) { + plantPosition = {1200, 700}; + } + else if (sp == TAKE_3_PLANT_BOTTOM_1) { + plantPosition = {950, 1300}; + } + else if (sp == TAKE_3_PLANT_BOTTOM_2) { + plantPosition = {1200, 1300}; + } + else { + return; + } } - else if (sp == TAKE_3_PLANT_TOP_2) { - plantPosition = {1200, 700}; - angle = 0; - direction = 1; + else if (team == YELLOW) { + angle = -PI; + direction = -1; + + if (sp == TAKE_3_PLANT_TOP_1) { + plantPosition = {2050, 700}; + } + else if (sp == TAKE_3_PLANT_TOP_2) { + plantPosition = {1800, 700}; + } + else if (sp == TAKE_3_PLANT_BOTTOM_1) { + plantPosition = {2050, 1300}; + } + else if (sp == TAKE_3_PLANT_BOTTOM_2) { + plantPosition = {1800, 1300}; + } + else { + return; + } } - else if (sp == TAKE_3_PLANT_BOTTOM_1) { - plantPosition = {950, 1300}; - angle = 0; - direction = 1; - } else if (sp == TAKE_3_PLANT_BOTTOM_2) { - plantPosition = {1200, 1300}; - angle = 0; - direction = 1; - } else { + else { return; } this->setMaxSpeed(); - this->transit(plantPosition[0]-400, plantPosition[1], 150); + this->transit(plantPosition[0]-600, plantPosition[1], 150); awaitRobotIdle(); - this->setSpeed(150); this->rotate(angle); awaitRobotIdle(); this->setMaxSpeed(); + this->go(plantPosition[0]-400, plantPosition[1]); + awaitRobotIdle(); + + this->setSpeed(150); + + this->rotate(angle); + awaitRobotIdle(); + + this->setMaxSpeed(); + + // TODO Check for falling flowers + for (int i = 0; i < 3; i++) { this->openPince(i); } usleep(200'000); - this->transit(plantPosition[0], this->robotPose.pos.y, 130); + this->go(plantPosition); awaitRobotIdle(); usleep(500'000); diff --git a/TCPServer.h b/TCPServer.h index ce5a951..90a8471 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -17,7 +17,7 @@ #include "utils.h" -#define MAX_SPEED 170 +#define MAX_SPEED 200 #define MIN_SPEED 150 struct ClientTCP @@ -114,22 +114,22 @@ private: Team team; std::vector stratPatterns = { - // TURN_SOLAR_PANNEL_1, - // TURN_SOLAR_PANNEL_2, - // TURN_SOLAR_PANNEL_3, - // CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, + TURN_SOLAR_PANNEL_1, + TURN_SOLAR_PANNEL_2, + TURN_SOLAR_PANNEL_3, + CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, GET_LIDAR_POS, /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - // TAKE_3_PLANT_BOTTOM_1, - // DROP_FLOWER_BASE_2, - // TAKE_3_PLANT_BOTTOM_2, - // REMOVE_POT_J2, - // DROP_FLOWER_J2, - // GET_LIDAR_POS, + TAKE_3_PLANT_BOTTOM_1, + DROP_FLOWER_BASE_2, + TAKE_3_PLANT_BOTTOM_2, + REMOVE_POT_J2, + DROP_FLOWER_J2, + GET_LIDAR_POS, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ @@ -138,8 +138,8 @@ private: TAKE_FLOWER_TOP, TAKE_FLOWER_TOP,*/ - // TAKE_3_PLANT_TOP_1, - // DROP_FLOWER_J1, + TAKE_3_PLANT_TOP_1, + DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J2,*/ @@ -151,10 +151,10 @@ private: /*DROP_WHITE_FLOWER_J2, DROP_PURPLE_FLOWER,*/ - // TAKE_3_PLANT_TOP_2, - // DROP_FLOWER_BASE_1, + TAKE_3_PLANT_TOP_2, + DROP_FLOWER_BASE_1, - // GO_END + GO_END }; // This is the index of the current pattern From dbeb13d37c7dbaf38d60565c84783b5d149f978a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sun, 5 May 2024 22:13:51 +0200 Subject: [PATCH 188/316] do not take fallen flower --- TCPServer.cpp | 36 +++++++++++++++++++++++++++++++++++- TCPServer.h | 2 ++ utils.h | 2 +- 3 files changed, 38 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3e157f7..df7dbaa 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -842,6 +842,29 @@ std::optional TCPServer::getMostCenteredArucoTag(const float borneMinX return found ? std::optional(mostCenteredTag) : std::nullopt; } +std::vector TCPServer::getNotFallenFlowers(float BorneMin, float BorneMax) { + std::vector res = {0, 1, 2}; + for (auto & tag : arucoTags) { + if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) { + auto roll = tag.rot()[1]; + auto yPos = tag.pos()[1]; + + if (roll < BorneMin && roll > BorneMax) { + if (yPos > 70) { + res[2] = -1; + } + else if (yPos < -70) { + res[0] = -1; + } + else { + res[1] = -1; + } + } + } + } + return res; +} + void TCPServer::handleEmergency(int distance, double angle) { this->handleEmergencyFlag = true; @@ -1469,8 +1492,19 @@ void TCPServer::go3Plants(const StratPattern sp) { // TODO Check for falling flowers + this->arucoTags.clear(); + for (int i = 0; i < 5; i++) { + this->broadcastMessage("strat;aruco;get aruco;1\n"); + usleep(110'000); + } + + // TODO, Les bornes ici est l'angle que la plante doit avoir pour etre considére comme tombée (par rapport a la caméra) + std::vector pinceCanTakeFLower = getNotFallenFlowers(0.3, 0.5); + for (int i = 0; i < 3; i++) { - this->openPince(i); + if (pinceCanTakeFLower[i] != -1) { + this->openPince(i); + } } usleep(200'000); diff --git a/TCPServer.h b/TCPServer.h index 90a8471..76e6f24 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -215,6 +215,8 @@ public: std::optional getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); + std::vector getNotFallenFlowers(float BorneMin, float BorneMax); + void handleEmergency(int distance, double angle); /* diff --git a/utils.h b/utils.h index ce106bc..60e5948 100644 --- a/utils.h +++ b/utils.h @@ -64,7 +64,7 @@ private: std::string _name; std::array _pos = {0, 0}; std::array _rot = {0, 0, 0}; - int nbFind = 0; + int nbFind = 1; }; double distanceToTag(const ArucoTag& tag); \ No newline at end of file From 6442a0a7db802f94ddba7b35d86ae9543423a509 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sun, 5 May 2024 22:15:56 +0200 Subject: [PATCH 189/316] do not take fallen flower --- TCPServer.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index df7dbaa..609632e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -847,13 +847,16 @@ std::vector TCPServer::getNotFallenFlowers(float BorneMin, float BorneMax) for (auto & tag : arucoTags) { if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) { auto roll = tag.rot()[1]; + auto xPos = tag.pos()[0]; auto yPos = tag.pos()[1]; if (roll < BorneMin && roll > BorneMax) { - if (yPos > 70) { + if (xPos > 800) continue; + + if (yPos > 70 && yPos < 200) { res[2] = -1; } - else if (yPos < -70) { + else if (yPos < -70 && yPos > -200) { res[0] = -1; } else { From 797db2a94d25b7b3c53704c06be699214f6d4f78 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 10:29:09 +0200 Subject: [PATCH 190/316] add lidar game mode --- TCPServer.cpp | 13 +++++++++++++ TCPServer.h | 3 ++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 609632e..725e604 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -212,6 +212,16 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) finishPoint[2] = PI / 2; break; + case 0: + this->team = LIDAR; + spawnPoint[0] = 500; + spawnPoint[1] = 1000; + spawnPoint[2] = 0; + + finishPoint[0] = 400; + finishPoint[1] = 500; + finishPoint[2] = PI / 2; + break; default: this->team = TEST; spawnPoint[0] = 1200; @@ -260,6 +270,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) case TEST: this->gameThread = std::thread([this]() { this->startGameTest(); }); break; + case LIDAR: + this->stratPatterns = { GET_LIDAR_POS }; + this->gameThread = std::thread([this]() { this->startGame(); }); } this->gameThread.detach(); diff --git a/TCPServer.h b/TCPServer.h index 76e6f24..0e4de91 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -34,7 +34,8 @@ struct ClientTCP enum Team { BLUE, YELLOW, - TEST + TEST, + LIDAR }; enum StratPattern { From 69bacd53bdeab16cb18f750d5916a4f316fba41d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 10:29:28 +0200 Subject: [PATCH 191/316] remove pause --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 725e604..f86882a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1585,8 +1585,6 @@ void TCPServer::removePot(StratPattern sp) { void TCPServer::getLidarPos() { - usleep(10'000'000); - this->askLidarPosition(); awaitForLidar = true; From 0acd7853d767e23ec44bf27315b583df55130c44 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 10:31:00 +0200 Subject: [PATCH 192/316] add sleep statement --- TCPServer.cpp | 11 ++++++++++- TCPServer.h | 3 +++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f86882a..e9edc7e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -271,7 +271,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->gameThread = std::thread([this]() { this->startGameTest(); }); break; case LIDAR: - this->stratPatterns = { GET_LIDAR_POS }; + this->stratPatterns = { SLEEP_5S, GET_LIDAR_POS }; this->gameThread = std::thread([this]() { this->startGame(); }); } @@ -520,6 +520,15 @@ void TCPServer::startGame() { case DROP_FLOWER_BASE_2: dropBaseFlowers(DROP_FLOWER_BASE_2); break; + case SLEEP_1S: + usleep(1'000'000); + break; + case SLEEP_5S: + usleep(5'000'000); + break; + case SLEEP_10S: + usleep(10'000'000); + break; } whereAmI++; } diff --git a/TCPServer.h b/TCPServer.h index 0e4de91..16a22e0 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -60,6 +60,9 @@ enum StratPattern { REMOVE_POT_J2, DROP_FLOWER_BASE_1, DROP_FLOWER_BASE_2, + SLEEP_1S, + SLEEP_5S, + SLEEP_10S, }; class TCPServer; // Forward declaration From 013301c4b15ed9970a985574240ef161507b8b2c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 17:51:11 +0200 Subject: [PATCH 193/316] fallen flower --- TCPServer.cpp | 6 +++--- TCPServer.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e9edc7e..33a072e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -864,7 +864,7 @@ std::optional TCPServer::getMostCenteredArucoTag(const float borneMinX return found ? std::optional(mostCenteredTag) : std::nullopt; } -std::vector TCPServer::getNotFallenFlowers(float BorneMin, float BorneMax) { +std::vector TCPServer::getNotFallenFlowers() { std::vector res = {0, 1, 2}; for (auto & tag : arucoTags) { if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) { @@ -872,7 +872,7 @@ std::vector TCPServer::getNotFallenFlowers(float BorneMin, float BorneMax) auto xPos = tag.pos()[0]; auto yPos = tag.pos()[1]; - if (roll < BorneMin && roll > BorneMax) { + if (roll > 2.7f && roll < -2.f) { if (xPos > 800) continue; if (yPos > 70 && yPos < 200) { @@ -1524,7 +1524,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } // TODO, Les bornes ici est l'angle que la plante doit avoir pour etre considére comme tombée (par rapport a la caméra) - std::vector pinceCanTakeFLower = getNotFallenFlowers(0.3, 0.5); + std::vector pinceCanTakeFLower = getNotFallenFlowers(); for (int i = 0; i < 3; i++) { if (pinceCanTakeFLower[i] != -1) { diff --git a/TCPServer.h b/TCPServer.h index 16a22e0..2455f24 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -219,7 +219,7 @@ public: std::optional getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); - std::vector getNotFallenFlowers(float BorneMin, float BorneMax); + std::vector getNotFallenFlowers(); void handleEmergency(int distance, double angle); From 2380a386f707a100d6e81a98a6c582d5366d461d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 17:51:23 +0200 Subject: [PATCH 194/316] const --- TCPServer.cpp | 2 +- TCPServer.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 33a072e..9c36b34 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -864,7 +864,7 @@ std::optional TCPServer::getMostCenteredArucoTag(const float borneMinX return found ? std::optional(mostCenteredTag) : std::nullopt; } -std::vector TCPServer::getNotFallenFlowers() { +std::vector TCPServer::getNotFallenFlowers() const { std::vector res = {0, 1, 2}; for (auto & tag : arucoTags) { if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) { diff --git a/TCPServer.h b/TCPServer.h index 2455f24..2a342e1 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -219,7 +219,7 @@ public: std::optional getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); - std::vector getNotFallenFlowers(); + std::vector getNotFallenFlowers() const; void handleEmergency(int distance, double angle); From eeca9c20d53f43d2bfde2a5fdadc41daf3be9412 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 18:10:32 +0200 Subject: [PATCH 195/316] remove lidar for the moment --- TCPServer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 2a342e1..3a1a340 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -122,7 +122,7 @@ private: TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, - GET_LIDAR_POS, + // GET_LIDAR_POS, /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, @@ -133,7 +133,7 @@ private: TAKE_3_PLANT_BOTTOM_2, REMOVE_POT_J2, DROP_FLOWER_J2, - GET_LIDAR_POS, + // GET_LIDAR_POS, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ From 81657dca0a58cd544f66a72f0ec82f2743bc2ab7 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 18:13:52 +0200 Subject: [PATCH 196/316] fix --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9c36b34..d8a0497 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -273,6 +273,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) case LIDAR: this->stratPatterns = { SLEEP_5S, GET_LIDAR_POS }; this->gameThread = std::thread([this]() { this->startGame(); }); + break; } this->gameThread.detach(); From d5417b53c91ab391dd2f99a52d21c3e69a195dfc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 18:49:21 +0200 Subject: [PATCH 197/316] baisser bras --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index d8a0497..07bf0a2 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1501,6 +1501,8 @@ void TCPServer::go3Plants(const StratPattern sp) { this->transit(plantPosition[0]-600, plantPosition[1], 150); awaitRobotIdle(); + this->baisserBras(); + this->rotate(angle); awaitRobotIdle(); From b8b5f11b4d402166d8a23f09ef23baceb7560dcc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 19:00:14 +0200 Subject: [PATCH 198/316] sleep --- TCPServer.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 07bf0a2..ec30ed5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -789,7 +789,7 @@ void TCPServer::awaitRobotIdle() { int timeout = 0; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop - usleep(100'000); + usleep(50'000); while (isRobotIdle < 3) { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); @@ -799,7 +799,6 @@ void TCPServer::awaitRobotIdle() { break; } } - usleep(50'000); } void TCPServer::handleArucoTag(const ArucoTag &tag) { @@ -1508,7 +1507,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->go(plantPosition[0]-400, plantPosition[1]); + this->go(plantPosition[0]-400, this->robotPose.pos.y); awaitRobotIdle(); this->setSpeed(150); @@ -1518,15 +1517,12 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - // TODO Check for falling flowers - this->arucoTags.clear(); for (int i = 0; i < 5; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); } - // TODO, Les bornes ici est l'angle que la plante doit avoir pour etre considére comme tombée (par rapport a la caméra) std::vector pinceCanTakeFLower = getNotFallenFlowers(); for (int i = 0; i < 3; i++) { @@ -1536,7 +1532,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(200'000); - this->go(plantPosition); + this->go(plantPosition[0], this->robotPose.pos.y); awaitRobotIdle(); usleep(500'000); From 36bf84a8c77ed96080e5571c1aed9326966f3038 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 19:00:56 +0200 Subject: [PATCH 199/316] robot idle --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index ec30ed5..954e117 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -790,7 +790,7 @@ void TCPServer::awaitRobotIdle() { // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop usleep(50'000); - while (isRobotIdle < 3) { + while (isRobotIdle < 2) { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; From 394926f5073e4ed0eaf9a885b994b73835ab6c55 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 19:03:25 +0200 Subject: [PATCH 200/316] stop robot for lidar --- TCPServer.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 954e117..ab9e098 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -868,11 +868,11 @@ std::vector TCPServer::getNotFallenFlowers() const { std::vector res = {0, 1, 2}; for (auto & tag : arucoTags) { if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) { - auto roll = tag.rot()[1]; + auto angle = tag.rot()[0]; auto xPos = tag.pos()[0]; auto yPos = tag.pos()[1]; - if (roll > 2.7f && roll < -2.f) { + if (angle > 2.7f && angle < -2.f) { if (xPos > 800) continue; if (yPos > 70 && yPos < 200) { @@ -1593,6 +1593,8 @@ void TCPServer::removePot(StratPattern sp) { void TCPServer::getLidarPos() { + this->broadcastMessage("strat;arduino;clear;1\n"); + this->askLidarPosition(); awaitForLidar = true; From 2ca1550fe89471fe27abb39f6d7aaa4f007e3148 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 19:06:23 +0200 Subject: [PATCH 201/316] plant position --- TCPServer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index ab9e098..825df7c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1456,13 +1456,13 @@ void TCPServer::go3Plants(const StratPattern sp) { angle = 0; direction = 1; if (sp == TAKE_3_PLANT_TOP_1) { - plantPosition = {950, 700}; + plantPosition = {900, 700}; } else if (sp == TAKE_3_PLANT_TOP_2) { plantPosition = {1200, 700}; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { - plantPosition = {950, 1300}; + plantPosition = {900, 1300}; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { plantPosition = {1200, 1300}; @@ -1476,13 +1476,13 @@ void TCPServer::go3Plants(const StratPattern sp) { direction = -1; if (sp == TAKE_3_PLANT_TOP_1) { - plantPosition = {2050, 700}; + plantPosition = {2100, 700}; } else if (sp == TAKE_3_PLANT_TOP_2) { plantPosition = {1800, 700}; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { - plantPosition = {2050, 1300}; + plantPosition = {2100, 1300}; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { plantPosition = {1800, 1300}; @@ -1507,7 +1507,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->go(plantPosition[0]-400, this->robotPose.pos.y); + this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); this->setSpeed(150); From c8230d69310f8a80150d9b56214081156a815e0c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 19:08:57 +0200 Subject: [PATCH 202/316] speed --- TCPServer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 825df7c..b3fa78b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1574,8 +1574,10 @@ void TCPServer::removePot(StratPattern sp) { if (sp == REMOVE_POT_J2) { this->go(230, 1000); awaitRobotIdle(); - this->go(210, 400); + this->setSpeed(150); + this->go(200, 400); awaitRobotIdle(); + this->setMaxSpeed(); this->go(230, 650); awaitRobotIdle(); } @@ -1583,8 +1585,10 @@ void TCPServer::removePot(StratPattern sp) { if (sp == REMOVE_POT_J2) { this->go(2770, 1000); awaitRobotIdle(); + this->setSpeed(150); this->go(2800, 400); awaitRobotIdle(); + this->setMaxSpeed(); this->go(2770, 650); awaitRobotIdle(); } From 7d2e813f0cf7f04e3e3f5114e4205a7e2c8790de Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 21:46:27 +0200 Subject: [PATCH 203/316] lidar --- TCPServer.cpp | 2 +- TCPServer.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b3fa78b..e7ac62f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -171,7 +171,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::vector args = TCPUtils::split(tokens[3], ","); this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), std::stof(args[2]) / 100}; this->setPosition(this->lidarCalculatePos); - usleep(100'000); + usleep(50'000); this->setPosition(this->lidarCalculatePos); awaitForLidar = false; } diff --git a/TCPServer.h b/TCPServer.h index 3a1a340..2a342e1 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -122,7 +122,7 @@ private: TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, - // GET_LIDAR_POS, + GET_LIDAR_POS, /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, @@ -133,7 +133,7 @@ private: TAKE_3_PLANT_BOTTOM_2, REMOVE_POT_J2, DROP_FLOWER_J2, - // GET_LIDAR_POS, + GET_LIDAR_POS, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ From b5545f851c3d4f90955b52f27df674a012cbc34e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 21:46:50 +0200 Subject: [PATCH 204/316] lidar --- TCPServer.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TCPServer.h b/TCPServer.h index 2a342e1..f4945a5 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -122,7 +122,6 @@ private: TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, - GET_LIDAR_POS, /*TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM, @@ -130,6 +129,8 @@ private: TAKE_3_PLANT_BOTTOM_1, DROP_FLOWER_BASE_2, + GET_LIDAR_POS, + TAKE_3_PLANT_BOTTOM_2, REMOVE_POT_J2, DROP_FLOWER_J2, From 2bfb771462d3c50594b477acba77ab053733925a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Mon, 6 May 2024 23:07:23 +0200 Subject: [PATCH 205/316] lidar | detect aruco --- TCPServer.cpp | 142 ++++++++++++++++++++++++++++---------------------- TCPServer.h | 7 ++- 2 files changed, 82 insertions(+), 67 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e7ac62f..719e1cb 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -167,9 +167,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); } else if (tokens[0] == "lidar" && tokens[2] == "set pos") { - this->broadcastMessage(tokens[0] + ";client;" + tokens[2] + ";" + tokens[3] + "\n"); std::vector args = TCPUtils::split(tokens[3], ","); - this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), std::stof(args[2]) / 100}; + // TODO replace angle with the real angle calculated by the lidar when working + this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), /*std::stof(args[2]) / 100*/ this->robotPose.theta}; this->setPosition(this->lidarCalculatePos); usleep(50'000); this->setPosition(this->lidarCalculatePos); @@ -183,26 +183,27 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) switch (spawnPointNb) { case 3: - this->team = BLUE; + this->setTeam(BLUE); spawnPoint[0] = 250; spawnPoint[1] = 1800; spawnPoint[2] = 0; + finishPoint[0] = 400; + finishPoint[1] = 500; + finishPoint[2] = PI / 2; + + // For test + /*spawnPoint[0] = 500; spawnPoint[1] = 1000; spawnPoint[2] = 0;*/ - // For test /*finishPoint[0] = 400; finishPoint[1] = 1790; finishPoint[2] = 0;*/ - - finishPoint[0] = 400; - finishPoint[1] = 500; - finishPoint[2] = PI / 2; break; case 6: - this->team = YELLOW; + this->setTeam(YELLOW); spawnPoint[0] = 1750; spawnPoint[1] = 1800; spawnPoint[2] = PI; @@ -212,16 +213,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) finishPoint[2] = PI / 2; break; - case 0: - this->team = LIDAR; - spawnPoint[0] = 500; - spawnPoint[1] = 1000; - spawnPoint[2] = 0; - - finishPoint[0] = 400; - finishPoint[1] = 500; - finishPoint[2] = PI / 2; - break; default: this->team = TEST; spawnPoint[0] = 1200; @@ -247,7 +238,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) usleep(1'000'000); } - this->broadcastMessage("strat;lidar;set team;" + std::to_string((this->team == BLUE ? 0 : 1)) + "\n"); + this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); this->broadcastMessage("strat;lidar;set beacon;1\n"); } else if (tokens[1] == "strat" && tokens[2] == "start") @@ -270,10 +261,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) case TEST: this->gameThread = std::thread([this]() { this->startGameTest(); }); break; - case LIDAR: - this->stratPatterns = { SLEEP_5S, GET_LIDAR_POS }; - this->gameThread = std::thread([this]() { this->startGame(); }); - break; } this->gameThread.detach(); @@ -410,7 +397,10 @@ void TCPServer::stop() { for (auto& thread : clientThreads) { thread.join(); } - this->gameThread.join(); + + if (gameStarted) { + this->gameThread.~thread(); + } // Close the server socket close(serverSocket); @@ -864,25 +854,38 @@ std::optional TCPServer::getMostCenteredArucoTag(const float borneMinX return found ? std::optional(mostCenteredTag) : std::nullopt; } -std::vector TCPServer::getNotFallenFlowers() const { - std::vector res = {0, 1, 2}; +std::vector TCPServer::getNotFallenFlowers() const { + std::vector res = {FLOWER, FLOWER, FLOWER}; for (auto & tag : arucoTags) { if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) { auto angle = tag.rot()[0]; auto xPos = tag.pos()[0]; auto yPos = tag.pos()[1]; - if (angle > 2.7f && angle < -2.f) { - if (xPos > 800) continue; + if (xPos > 700) continue; - if (yPos > 70 && yPos < 200) { - res[2] = -1; - } - else if (yPos < -70 && yPos > -200) { - res[0] = -1; + if (yPos > 70 && yPos < 200) { + if (angle > 2.7f && angle < -2.f) { + res[2] = NONE; } else { - res[1] = -1; + res[2] = (TCPUtils::contains(tag.name(), "White") ? WHITE_FLOWER : PURPLE_FLOWER); + } + } + else if (yPos < -70 && yPos > -200) { + if (angle > 2.7f && angle < -2.f) { + res[0] = NONE; + } + else { + res[0] = (TCPUtils::contains(tag.name(), "White") ? WHITE_FLOWER : PURPLE_FLOWER); + } + } + else { + if (angle > 2.7f && angle < -2.f) { + res[1] = NONE; + } + else { + res[1] = (TCPUtils::contains(tag.name(), "White") ? WHITE_FLOWER : PURPLE_FLOWER); } } } @@ -897,13 +900,18 @@ void TCPServer::handleEmergency(int distance, double angle) { usleep(2'000'000); this->stopEmergency = false; usleep(500'000); + // ReSharper disable once CppDFAConstantConditions if (this->stopEmergency) { // TODO here go back by twenty centimeter + // ReSharper disable once CppDFAUnreachableCode + double newAngle = this->robotPose.theta + angle; + double newX = this->robotPose.pos.x + 200 * std::cos(newAngle); + double newY = this->robotPose.pos.y + 200 * std::sin(newAngle); usleep(200'000); + this->go(newX, newY); + awaitRobotIdle(); } - this->handleEmergencyFlag = false; - this->gameThread.~thread(); this->gameStarted = false; @@ -911,6 +919,8 @@ void TCPServer::handleEmergency(int distance, double angle) { this->gameThread = std::thread([this]() { this->startGame(); }); this->gameThread.detach(); + + this->handleEmergencyFlag = false; } void TCPServer::startTestAruco(const int pince) { @@ -1327,24 +1337,17 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->go(whiteDropPosition); usleep(2'000'000); - // reposition, TODO remove when lidar is working - if (sp == DROP_FLOWER_J1) { - this->setPosition(this->robotPose.pos.x, 142, this->robotPose.theta); + if (pinceState[0] != NONE) { + this->fullyOpenPince(0); + pinceState[0] = NONE; + this->sendPoint(3+1); } - else if (sp == DROP_FLOWER_J2) { - if (team == BLUE) { - this->setPosition(142, this->robotPose.pos.y, this->robotPose.theta); - } - else if (team == YELLOW) { - this->setPosition(2858, this->robotPose.pos.y, this->robotPose.theta); - } + if (pinceState[2] != NONE) { + this->fullyOpenPince(2); + pinceState[2] = NONE; + this->sendPoint(3+1); } - this->fullyOpenPince(0); - this->fullyOpenPince(2); - pinceState[0] = NONE; - pinceState[2] = NONE; - usleep(500'000); this->closePince(0); @@ -1352,13 +1355,16 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { usleep(100'000); - this->fullyOpenPince(1); - pinceState[1] = NONE; + if (pinceState[1] != NONE) { + this->fullyOpenPince(1); + pinceState[1] = NONE; + this->sendPoint(3+1); + } usleep(500'000); - this->openPince(1); this->openPince(0); + this->openPince(1); this->openPince(2); this->setMaxSpeed(); @@ -1367,10 +1373,6 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { awaitRobotIdle(); this->transportBras(); - - this->sendPoint(4); - this->sendPoint(4); - this->sendPoint(4); } void TCPServer::dropBaseFlowers(StratPattern sp) { @@ -1435,12 +1437,21 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance); awaitRobotIdle(); + bool detectedPurple = false; + for (int i = 0; i < 3; i++) { + if (pinceState[i] == PURPLE_FLOWER) { + detectedPurple = true; + this->sendPoint(3); + } + pinceState[i] = NONE; this->closePince(i); } - this->sendPoint(3); + if (!detectedPurple) { + this->sendPoint(3); + } this->transportBras(); @@ -1523,10 +1534,10 @@ void TCPServer::go3Plants(const StratPattern sp) { usleep(110'000); } - std::vector pinceCanTakeFLower = getNotFallenFlowers(); + std::vector pinceCanTakeFLower = getNotFallenFlowers(); for (int i = 0; i < 3; i++) { - if (pinceCanTakeFLower[i] != -1) { + if (pinceCanTakeFLower[i] != NONE) { this->openPince(i); } } @@ -1539,7 +1550,7 @@ void TCPServer::go3Plants(const StratPattern sp) { for (int i = 0; i < 3; i++) { this->closePince(i); - pinceState[i] = FLOWER; + pinceState[i] = pinceCanTakeFLower[i]; } usleep(500'000); @@ -1765,3 +1776,8 @@ void TCPServer::askLidarPosition() { void TCPServer::sendPoint(int point) { this->broadcastMessage("strat;ihm;add point;" + std::to_string(point) + "\n"); } + +void TCPServer::setTeam(Team team) { + this->team = team; + this->broadcastMessage("strat;all;set team;" + std::to_string(team) + "\n"); +} diff --git a/TCPServer.h b/TCPServer.h index f4945a5..3b83457 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -10,8 +10,6 @@ #include #include #include -#include -#include #include #include @@ -35,7 +33,6 @@ enum Team { BLUE, YELLOW, TEST, - LIDAR }; enum StratPattern { @@ -220,7 +217,7 @@ public: std::optional getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY); - std::vector getNotFallenFlowers() const; + std::vector getNotFallenFlowers() const; void handleEmergency(int distance, double angle); @@ -314,5 +311,7 @@ public: void sendPoint(int point); + void setTeam(Team team); + ~TCPServer(); }; From 8f8500dc9e1cca5a99cfb3c6becf47f98fb5b764 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:14:21 +0200 Subject: [PATCH 206/316] not timeout --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 719e1cb..65c5c1e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -783,11 +783,11 @@ void TCPServer::awaitRobotIdle() { while (isRobotIdle < 2) { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); - timeout++; + /*timeout++; if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; - } + }*/ } } From 3f8151a562c3bd42a89cbd7e80d78e659701b547 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:27:38 +0200 Subject: [PATCH 207/316] not timeout --- TCPServer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 65c5c1e..3e204ab 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1676,7 +1676,8 @@ void TCPServer::setSpeed(const int speed) { } void TCPServer::setMaxSpeed() { - this->setSpeed(MAX_SPEED); + // this->setSpeed(MAX_SPEED); + this->setSpeed(200); } void TCPServer::setMinSpeed() { From 25b26ab65520777283992ea0ab738c1a570b064e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:31:54 +0200 Subject: [PATCH 208/316] remove stop proximity --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3e204ab..f5c9540 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -125,7 +125,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (tokens[2] == "stop proximity") { if (!gameStarted) return; - this->stopEmergency = true; + /*this->stopEmergency = true; this->gameThread.~thread(); this->broadcastMessage("strat;arduino;clear;1"); @@ -135,7 +135,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) 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); From 2bb0da828fb0c2ca59c4295e243cc30e03e596ed Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:38:05 +0200 Subject: [PATCH 209/316] lidar pos --- TCPServer.cpp | 4 ++++ TCPServer.h | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f5c9540..4a57242 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1610,6 +1610,10 @@ void TCPServer::getLidarPos() { this->broadcastMessage("strat;arduino;clear;1\n"); + usleep(1'000'000); + + this->setPosition(this->robotPose, this->lidarSocket); + this->askLidarPosition(); awaitForLidar = true; diff --git a/TCPServer.h b/TCPServer.h index 3b83457..aad2b57 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -126,12 +126,11 @@ private: TAKE_3_PLANT_BOTTOM_1, DROP_FLOWER_BASE_2, - GET_LIDAR_POS, TAKE_3_PLANT_BOTTOM_2, + GET_LIDAR_POS, REMOVE_POT_J2, DROP_FLOWER_J2, - GET_LIDAR_POS, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ @@ -141,6 +140,7 @@ private: TAKE_FLOWER_TOP,*/ TAKE_3_PLANT_TOP_1, + GET_LIDAR_POS, DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, From 2e54200ef1914c5fabfa6097839c51c3c3629aca Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:40:46 +0200 Subject: [PATCH 210/316] sleep --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4a57242..c7a6158 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -235,7 +235,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) for (int j = 0; j < 3; j++) { this->setPosition(this->initRobotPose); - usleep(1'000'000); + usleep(100'000); } this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); From 07be507b31b01a14cd3db41e55a43dacb8ad77ad Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:45:48 +0200 Subject: [PATCH 211/316] lidar --- TCPServer.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.h b/TCPServer.h index aad2b57..12c024c 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -115,6 +115,7 @@ private: Team team; std::vector stratPatterns = { + GET_LIDAR_POS, TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, @@ -124,6 +125,7 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ + GET_LIDAR_POS, TAKE_3_PLANT_BOTTOM_1, DROP_FLOWER_BASE_2, From 4040493906a78c446d503c20cbb49476dd8f2d0f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:48:32 +0200 Subject: [PATCH 212/316] already there --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c7a6158..2331541 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -251,8 +251,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->setSpeed(200); - this->go(this->initRobotPose.pos.x, this->initRobotPose.pos.y); - switch (this->team) { case BLUE: case YELLOW: From c3d48bda6d74e4e69156f1d199249564f79f4192 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:54:08 +0200 Subject: [PATCH 213/316] lidar --- TCPServer.h | 1 - 1 file changed, 1 deletion(-) diff --git a/TCPServer.h b/TCPServer.h index 12c024c..64014fe 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -115,7 +115,6 @@ private: Team team; std::vector stratPatterns = { - GET_LIDAR_POS, TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, From 9b0680fe3e5459281f4fff03961bc1529465a724 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 11:59:28 +0200 Subject: [PATCH 214/316] check if work without lidar --- TCPServer.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 64014fe..ba700f0 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -124,12 +124,12 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - GET_LIDAR_POS, + // GET_LIDAR_POS, TAKE_3_PLANT_BOTTOM_1, DROP_FLOWER_BASE_2, TAKE_3_PLANT_BOTTOM_2, - GET_LIDAR_POS, + // GET_LIDAR_POS, REMOVE_POT_J2, DROP_FLOWER_J2, @@ -141,7 +141,7 @@ private: TAKE_FLOWER_TOP,*/ TAKE_3_PLANT_TOP_1, - GET_LIDAR_POS, + // GET_LIDAR_POS, DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, From 040078faf9510e501148d1df6eae75f62ef1cf80 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 14:45:51 +0200 Subject: [PATCH 215/316] speed | sleep --- TCPServer.cpp | 7 ++++--- TCPServer.h | 5 ++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2331541..eef70df 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1612,13 +1612,15 @@ void TCPServer::getLidarPos() { this->setPosition(this->robotPose, this->lidarSocket); + usleep(100'000); + this->askLidarPosition(); awaitForLidar = true; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop while (awaitForLidar) { - usleep(100'000); + usleep(50'000); } // ReSharper disable once CppDFAUnreachableCode @@ -1678,8 +1680,7 @@ void TCPServer::setSpeed(const int speed) { } void TCPServer::setMaxSpeed() { - // this->setSpeed(MAX_SPEED); - this->setSpeed(200); + this->setSpeed(MAX_SPEED); } void TCPServer::setMinSpeed() { diff --git a/TCPServer.h b/TCPServer.h index ba700f0..aad2b57 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -124,12 +124,11 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - // GET_LIDAR_POS, TAKE_3_PLANT_BOTTOM_1, DROP_FLOWER_BASE_2, TAKE_3_PLANT_BOTTOM_2, - // GET_LIDAR_POS, + GET_LIDAR_POS, REMOVE_POT_J2, DROP_FLOWER_J2, @@ -141,7 +140,7 @@ private: TAKE_FLOWER_TOP,*/ TAKE_3_PLANT_TOP_1, - // GET_LIDAR_POS, + GET_LIDAR_POS, DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, From e7a5b99ac188659fd04bd1d2ea8800b1375997d6 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 15:06:56 +0200 Subject: [PATCH 216/316] solar panel --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index eef70df..0932292 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1222,7 +1222,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(300'000); + usleep(700'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: @@ -1233,7 +1233,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(300'000); + usleep(700'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: @@ -1244,7 +1244,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(300'000); + usleep(700'000); this->uncheckPanneau(7); break; default: @@ -1260,7 +1260,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(300'000); + usleep(700'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: @@ -1271,7 +1271,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(300'000); + usleep(700'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: @@ -1282,7 +1282,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(300'000); + usleep(700'000); this->uncheckPanneau(6); break; default: From a0b0b9f59c499e6bc184b62f8a30040c1ba652c1 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 15:25:35 +0200 Subject: [PATCH 217/316] lidar --- TCPServer.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 0932292..ce2cf40 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -295,7 +295,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::vector pos = TCPUtils::split(tokens[3], ","); this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; if (!awaitForLidar) { - this->setPosition(this->robotPose, lidarSocket); + std::string toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast(this->robotPose.theta * 100)) + "\n"; + this->broadcastMessage(toSend, clientSocket); } } } else if (tokens[2] == "test aruco") { @@ -765,10 +766,8 @@ void TCPServer::askArduinoPos() { } while (!this->_shouldStop) { - if (!awaitForLidar) { - this->sendToClient("strat;arduino;get pos;1\n", this->arduinoSocket); - }; - usleep(200'000); + this->sendToClient("strat;arduino;get pos;1\n", this->arduinoSocket); + usleep(100'000); } } @@ -1719,7 +1718,7 @@ void TCPServer::setPosition(const Position pos, const int clientSocket) { if (clientSocket == -1) { this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n"); } else { - this->sendToClient("strat;all;set pos;" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n", clientSocket); + this->sendToClient("strat;lidar;set pos;" + std::to_string(static_cast(pos.pos.x)) + "," + std::to_string(static_cast(pos.pos.y)) + "," + std::to_string(static_cast(pos.theta * 100)) + "\n", clientSocket); } } From cc2b211470d1deb4c536faa0444f8767c77235d5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 15:44:22 +0200 Subject: [PATCH 218/316] lidar --- TCPServer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index ce2cf40..6bb8e01 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -151,7 +151,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) { client.isReady = true; client.socket = clientSocket; - if (client.name == "lidar") { + if (TCPUtils::contains(client.name, "lidar")) { this->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) // TODO replace angle with the real angle calculated by the lidar when working this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), /*std::stof(args[2]) / 100*/ this->robotPose.theta}; this->setPosition(this->lidarCalculatePos); - usleep(50'000); + usleep(100'000); this->setPosition(this->lidarCalculatePos); awaitForLidar = false; } @@ -1613,9 +1613,10 @@ void TCPServer::getLidarPos() { usleep(100'000); + awaitForLidar = true; + this->askLidarPosition(); - awaitForLidar = true; // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop while (awaitForLidar) { From 00767e30eb0ba26e6e1d7573667aa90140c6cb6e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 15:51:10 +0200 Subject: [PATCH 219/316] lidar --- TCPServer.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.h b/TCPServer.h index aad2b57..7ce51f7 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -118,6 +118,7 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, + GET_LIDAR_POS, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, /*TAKE_FLOWER_BOTTOM, @@ -125,6 +126,7 @@ private: TAKE_FLOWER_BOTTOM,*/ TAKE_3_PLANT_BOTTOM_1, + GET_LIDAR_POS, DROP_FLOWER_BASE_2, TAKE_3_PLANT_BOTTOM_2, From 135f9d45d214ef5589e5dfb313ab14cbffdc7cbc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 15:59:15 +0200 Subject: [PATCH 220/316] lidar --- TCPServer.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6bb8e01..707074e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -295,7 +295,13 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::vector pos = TCPUtils::split(tokens[3], ","); this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; if (!awaitForLidar) { - std::string toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast(this->robotPose.theta * 100)) + "\n"; + std::string toSend; + + if (this->robotPose.theta < 0 ) { + toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast((this->robotPose.theta + 2 * PI) * 100)) + "\n"; + } else { + toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast(this->robotPose.theta * 100)) + "\n"; + } this->broadcastMessage(toSend, clientSocket); } } From f66120d5c515de489dc810157aaffc80aceb2fb5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:00:32 +0200 Subject: [PATCH 221/316] lidar --- TCPServer.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 707074e..d84c995 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1615,7 +1615,16 @@ void TCPServer::getLidarPos() { usleep(1'000'000); - this->setPosition(this->robotPose, this->lidarSocket); + + std::string toSend; + if (this->robotPose.theta < 0) { + toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast((this->robotPose.theta + 2 * PI) * 100)) + "\n"; + } + else { + toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast(this->robotPose.theta * 100)) + "\n"; + } + + this->broadcastMessage(toSend); usleep(100'000); From 7b8da18cad96114a4ace9721c9a6227f536e5e52 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:05:45 +0200 Subject: [PATCH 222/316] sleep --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d84c995..a0148ae 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -773,7 +773,7 @@ void TCPServer::askArduinoPos() { while (!this->_shouldStop) { this->sendToClient("strat;arduino;get pos;1\n", this->arduinoSocket); - usleep(100'000); + usleep(20'000); } } From a8a013555a144d3d8469965fa95920fd080a6b0b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:10:06 +0200 Subject: [PATCH 223/316] lidar --- TCPServer.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 7ce51f7..aad2b57 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -118,7 +118,6 @@ private: TURN_SOLAR_PANNEL_1, TURN_SOLAR_PANNEL_2, TURN_SOLAR_PANNEL_3, - GET_LIDAR_POS, CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER, /*TAKE_FLOWER_BOTTOM, @@ -126,7 +125,6 @@ private: TAKE_FLOWER_BOTTOM,*/ TAKE_3_PLANT_BOTTOM_1, - GET_LIDAR_POS, DROP_FLOWER_BASE_2, TAKE_3_PLANT_BOTTOM_2, From eab75bd5c2f0326bea7ba443d95deea037ee8e22 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:13:51 +0200 Subject: [PATCH 224/316] remove pot --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a0148ae..4a4d52d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1586,24 +1586,24 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->go(230, 1000); + this->go(200, 1000); awaitRobotIdle(); this->setSpeed(150); - this->go(200, 400); + this->go(180, 400); awaitRobotIdle(); this->setMaxSpeed(); - this->go(230, 650); + this->go(220, 650); awaitRobotIdle(); } } else if (team == YELLOW) { if (sp == REMOVE_POT_J2) { - this->go(2770, 1000); + this->go(2800, 1000); awaitRobotIdle(); this->setSpeed(150); - this->go(2800, 400); + this->go(2820, 400); awaitRobotIdle(); this->setMaxSpeed(); - this->go(2770, 650); + this->go(2780, 650); awaitRobotIdle(); } } From 615fefba4e8d928c212e88a571946c2063661f3a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:17:09 +0200 Subject: [PATCH 225/316] remove pot --- TCPServer.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4a4d52d..7e02060 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1511,7 +1511,8 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->transit(plantPosition[0]-600, plantPosition[1], 150); + // this->transit(plantPosition[0]-600, plantPosition[1], 150); + this->go(plantPosition[0]-600, plantPosition[1]); awaitRobotIdle(); this->baisserBras(); @@ -1586,13 +1587,13 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->go(200, 1000); + this->go(220, 1000); awaitRobotIdle(); this->setSpeed(150); - this->go(180, 400); + this->go(200, 400); awaitRobotIdle(); this->setMaxSpeed(); - this->go(220, 650); + this->go(230, 650); awaitRobotIdle(); } } else if (team == YELLOW) { From a5e6a542a80e8bf2cdb49ceb39c94b46264c8b4d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:32:31 +0200 Subject: [PATCH 226/316] speed --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7e02060..c128580 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1547,9 +1547,11 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(200'000); - this->go(plantPosition[0], this->robotPose.pos.y); + this->transit(plantPosition[0], this->robotPose.pos.y, 130); awaitRobotIdle(); + this->setMaxSpeed(); + usleep(500'000); for (int i = 0; i < 3; i++) { From 5fae8a0f8d8bca29790df18f6f53d35273e53e8f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:38:10 +0200 Subject: [PATCH 227/316] position --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c128580..48a7b46 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1473,13 +1473,13 @@ void TCPServer::go3Plants(const StratPattern sp) { plantPosition = {900, 700}; } else if (sp == TAKE_3_PLANT_TOP_2) { - plantPosition = {1200, 700}; + plantPosition = {1100, 700}; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { plantPosition = {900, 1300}; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { - plantPosition = {1200, 1300}; + plantPosition = {1100, 1300}; } else { return; @@ -1512,7 +1512,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); // this->transit(plantPosition[0]-600, plantPosition[1], 150); - this->go(plantPosition[0]-600, plantPosition[1]); + this->go(plantPosition[0]-500, plantPosition[1]); awaitRobotIdle(); this->baisserBras(); @@ -1522,7 +1522,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->go(plantPosition[0]-400, plantPosition[1]); + this->go(plantPosition[0]-300, plantPosition[1]); awaitRobotIdle(); this->setSpeed(150); @@ -1589,10 +1589,10 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->go(220, 1000); + this->go(210, 1100); awaitRobotIdle(); this->setSpeed(150); - this->go(200, 400); + this->go(190, 400); awaitRobotIdle(); this->setMaxSpeed(); this->go(230, 650); From b5be1b9b80b9268ec1a8b0a96aa689eeb7ddd1cf Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:43:18 +0200 Subject: [PATCH 228/316] pot position --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 48a7b46..b89b94a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1589,9 +1589,11 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->go(210, 1100); + this->go(220, 1100); awaitRobotIdle(); this->setSpeed(150); + this->go(190, 900); + awaitRobotIdle(); this->go(190, 400); awaitRobotIdle(); this->setMaxSpeed(); From e39633267d201b1612fe3148ea4848a1685c43c4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:52:10 +0200 Subject: [PATCH 229/316] strat --- TCPServer.cpp | 2 +- TCPServer.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b89b94a..c3e4004 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1589,7 +1589,7 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->go(220, 1100); + this->go(230, 1100); awaitRobotIdle(); this->setSpeed(150); this->go(190, 900); diff --git a/TCPServer.h b/TCPServer.h index aad2b57..9b24b16 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -124,8 +124,8 @@ private: TAKE_FLOWER_BOTTOM, TAKE_FLOWER_BOTTOM,*/ - TAKE_3_PLANT_BOTTOM_1, - DROP_FLOWER_BASE_2, + /*TAKE_3_PLANT_BOTTOM_1, + DROP_FLOWER_BASE_2,*/ TAKE_3_PLANT_BOTTOM_2, GET_LIDAR_POS, From 1c66ea928a0285c66d77ada2a479c77b465a8ac7 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:52:34 +0200 Subject: [PATCH 230/316] plant --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c3e4004..d088de3 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1511,8 +1511,8 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - // this->transit(plantPosition[0]-600, plantPosition[1], 150); - this->go(plantPosition[0]-500, plantPosition[1]); + this->transit(plantPosition[0]-500, plantPosition[1], 170); + // this->go(plantPosition[0]-500, plantPosition[1]); awaitRobotIdle(); this->baisserBras(); From 0e8d8b98de8c616712d1649aa9f7398c23f9f7ee Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:54:51 +0200 Subject: [PATCH 231/316] plant --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d088de3..c4d2578 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1511,7 +1511,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->transit(plantPosition[0]-500, plantPosition[1], 170); + this->transit(plantPosition[0]-600, plantPosition[1], 170); // this->go(plantPosition[0]-500, plantPosition[1]); awaitRobotIdle(); @@ -1522,7 +1522,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->go(plantPosition[0]-300, plantPosition[1]); + this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); this->setSpeed(150); From f12df10d4d953cd29c11c4c19bfa1616c8116746 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 16:57:10 +0200 Subject: [PATCH 232/316] plant --- TCPServer.cpp | 4 ++-- TCPServer.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c4d2578..d088de3 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1511,7 +1511,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->transit(plantPosition[0]-600, plantPosition[1], 170); + this->transit(plantPosition[0]-500, plantPosition[1], 170); // this->go(plantPosition[0]-500, plantPosition[1]); awaitRobotIdle(); @@ -1522,7 +1522,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->go(plantPosition[0]-400, plantPosition[1]); + this->go(plantPosition[0]-300, plantPosition[1]); awaitRobotIdle(); this->setSpeed(150); diff --git a/TCPServer.h b/TCPServer.h index 9b24b16..795c8b6 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -127,7 +127,7 @@ private: /*TAKE_3_PLANT_BOTTOM_1, DROP_FLOWER_BASE_2,*/ - TAKE_3_PLANT_BOTTOM_2, + TAKE_3_PLANT_BOTTOM_1, GET_LIDAR_POS, REMOVE_POT_J2, DROP_FLOWER_J2, From 4186a3bb12766b832de6e79aa454e643d0709acd Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 17:01:53 +0200 Subject: [PATCH 233/316] transit --- TCPServer.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d088de3..c146c4e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1589,9 +1589,8 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->go(230, 1100); + this->transit(230, 1100, 150); awaitRobotIdle(); - this->setSpeed(150); this->go(190, 900); awaitRobotIdle(); this->go(190, 400); From a1c394d93747660e4cc679246051992782e02bc9 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 18:34:42 +0200 Subject: [PATCH 234/316] lidar patch --- TCPServer.cpp | 33 +++++++++++---------------------- 1 file changed, 11 insertions(+), 22 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c146c4e..59d1fbd 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -170,10 +170,15 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::vector args = TCPUtils::split(tokens[3], ","); // TODO replace angle with the real angle calculated by the lidar when working this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), /*std::stof(args[2]) / 100*/ this->robotPose.theta}; - this->setPosition(this->lidarCalculatePos); - usleep(100'000); - this->setPosition(this->lidarCalculatePos); - awaitForLidar = false; + if (lidarCalculatePos.pos.x == -1 || lidarCalculatePos.pos.y == -1) { + this->askLidarPosition(); + } + else { + this->setPosition(this->lidarCalculatePos); + usleep(100'000); + this->setPosition(this->lidarCalculatePos); + awaitForLidar = false; + } } else if (tokens[0] == "ihm") { if (tokens[2] == "spawn") { @@ -295,14 +300,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::vector pos = TCPUtils::split(tokens[3], ","); this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100}; if (!awaitForLidar) { - std::string toSend; - - if (this->robotPose.theta < 0 ) { - toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast((this->robotPose.theta + 2 * PI) * 100)) + "\n"; - } else { - toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast(this->robotPose.theta * 100)) + "\n"; - } - this->broadcastMessage(toSend, clientSocket); + this->setPosition(this->robotPose, lidarSocket); } } } else if (tokens[2] == "test aruco") { @@ -1619,16 +1617,7 @@ void TCPServer::getLidarPos() { usleep(1'000'000); - - std::string toSend; - if (this->robotPose.theta < 0) { - toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast((this->robotPose.theta + 2 * PI) * 100)) + "\n"; - } - else { - toSend = "strat;lidar;set pos;" + std::to_string(static_cast(this->robotPose.pos.x)) + "," + std::to_string(static_cast(this->robotPose.pos.y)) + "," + std::to_string(static_cast(this->robotPose.theta * 100)) + "\n"; - } - - this->broadcastMessage(toSend); + this->setPosition(this->robotPose, lidarSocket); usleep(100'000); From f8328879d3811b745b7fad6a798d7add841e7218 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 20:44:54 +0200 Subject: [PATCH 235/316] lidar patch --- TCPServer.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 59d1fbd..8d170e5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1307,8 +1307,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { whiteDropPosition = std::array{762, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - whiteDropSetup = std::array{300, 612}; - whiteDropPosition = std::array{0, 612}; + whiteDropSetup = std::array{300, 607}; + whiteDropPosition = std::array{0, 607}; angle = -PI; } } else if (team == YELLOW) { @@ -1317,8 +1317,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { whiteDropPosition = std::array{2237, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - whiteDropSetup = std::array{1700, 612}; - whiteDropPosition = std::array{0, 612}; + whiteDropSetup = std::array{1700, 607}; + whiteDropPosition = std::array{0, 607}; angle = 0; } } @@ -1491,13 +1491,13 @@ void TCPServer::go3Plants(const StratPattern sp) { plantPosition = {2100, 700}; } else if (sp == TAKE_3_PLANT_TOP_2) { - plantPosition = {1800, 700}; + plantPosition = {1900, 700}; } else if (sp == TAKE_3_PLANT_BOTTOM_1) { plantPosition = {2100, 1300}; } else if (sp == TAKE_3_PLANT_BOTTOM_2) { - plantPosition = {1800, 1300}; + plantPosition = {1900, 1300}; } else { return; @@ -1564,7 +1564,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->go(this->robotPose.pos.x-300, this->robotPose.pos.y); + this->go(this->robotPose.pos.x - (300 * direction), this->robotPose.pos.y); awaitRobotIdle(); for (int i = 0; i < 3; i++) { @@ -1599,13 +1599,14 @@ void TCPServer::removePot(StratPattern sp) { } } else if (team == YELLOW) { if (sp == REMOVE_POT_J2) { - this->go(2800, 1000); + this->transit(2770, 1100, 150); awaitRobotIdle(); - this->setSpeed(150); - this->go(2820, 400); + this->go(2810, 900); + awaitRobotIdle(); + this->go(2810, 400); awaitRobotIdle(); this->setMaxSpeed(); - this->go(2780, 650); + this->go(2770, 650); awaitRobotIdle(); } } From a69f57ac35af0da5d44ec0bc721b6f2136ee69e3 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 21:07:41 +0200 Subject: [PATCH 236/316] speed --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8d170e5..07dec25 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1523,8 +1523,6 @@ void TCPServer::go3Plants(const StratPattern sp) { this->go(plantPosition[0]-300, plantPosition[1]); awaitRobotIdle(); - this->setSpeed(150); - this->rotate(angle); awaitRobotIdle(); From b6dc3ac9416b5617ea55d16b9aa744be276f7977 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 21:08:33 +0200 Subject: [PATCH 237/316] pos --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 07dec25..3bbdf28 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1509,7 +1509,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->transit(plantPosition[0]-500, plantPosition[1], 170); + this->transit(plantPosition[0]-600, plantPosition[1], 170); // this->go(plantPosition[0]-500, plantPosition[1]); awaitRobotIdle(); @@ -1520,7 +1520,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->go(plantPosition[0]-300, plantPosition[1]); + this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); this->rotate(angle); From e3185f638885e30668b9e9b7cdeef0617ebd734c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 21:15:33 +0200 Subject: [PATCH 238/316] speed --- TCPServer.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 3bbdf28..5c96354 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1515,11 +1515,11 @@ void TCPServer::go3Plants(const StratPattern sp) { this->baisserBras(); + this->setMaxSpeed(); + this->rotate(angle); awaitRobotIdle(); - this->setMaxSpeed(); - this->go(plantPosition[0]-400, plantPosition[1]); awaitRobotIdle(); @@ -1556,7 +1556,6 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(500'000); - this->setSpeed(150); this->rotate(angle); awaitRobotIdle(); @@ -1587,11 +1586,11 @@ void TCPServer::removePot(StratPattern sp) { if (sp == REMOVE_POT_J2) { this->transit(230, 1100, 150); awaitRobotIdle(); + this->setMaxSpeed(); this->go(190, 900); awaitRobotIdle(); this->go(190, 400); awaitRobotIdle(); - this->setMaxSpeed(); this->go(230, 650); awaitRobotIdle(); } @@ -1599,11 +1598,11 @@ void TCPServer::removePot(StratPattern sp) { if (sp == REMOVE_POT_J2) { this->transit(2770, 1100, 150); awaitRobotIdle(); + this->setMaxSpeed(); this->go(2810, 900); awaitRobotIdle(); this->go(2810, 400); awaitRobotIdle(); - this->setMaxSpeed(); this->go(2770, 650); awaitRobotIdle(); } From f176249605bef60d9f769b467ef65eac11ada52e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 21:34:04 +0200 Subject: [PATCH 239/316] pos --- TCPServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5c96354..a79fb5f 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1584,12 +1584,12 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->transit(230, 1100, 150); + this->transit(220, 1100, 150); awaitRobotIdle(); this->setMaxSpeed(); - this->go(190, 900); + this->go(185, 900); awaitRobotIdle(); - this->go(190, 400); + this->go(185, 400); awaitRobotIdle(); this->go(230, 650); awaitRobotIdle(); From 5b28181e73bc1ba14c5c11e339cc9567f6fd133b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 21:40:41 +0200 Subject: [PATCH 240/316] pos --- TCPServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a79fb5f..2aae114 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1584,12 +1584,12 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->transit(220, 1100, 150); + this->transit(230, 1100, 150); awaitRobotIdle(); this->setMaxSpeed(); - this->go(185, 900); + this->go(190, 900); awaitRobotIdle(); - this->go(185, 400); + this->go(180, 400); awaitRobotIdle(); this->go(230, 650); awaitRobotIdle(); From f7db4641918aefcf3efe1a66d839585fe3f751cd Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 21:42:12 +0200 Subject: [PATCH 241/316] add emergency --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2aae114..52b86bd 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -125,7 +125,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (tokens[2] == "stop proximity") { if (!gameStarted) return; - /*this->stopEmergency = true; + this->stopEmergency = true; this->gameThread.~thread(); this->broadcastMessage("strat;arduino;clear;1"); @@ -135,7 +135,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) 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); @@ -784,11 +784,11 @@ void TCPServer::awaitRobotIdle() { while (isRobotIdle < 2) { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); - /*timeout++; + timeout++; if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; - }*/ + } } } @@ -905,12 +905,12 @@ void TCPServer::handleEmergency(int distance, double angle) { if (this->stopEmergency) { // TODO here go back by twenty centimeter // ReSharper disable once CppDFAUnreachableCode - double newAngle = this->robotPose.theta + angle; + /*double newAngle = this->robotPose.theta + angle; double newX = this->robotPose.pos.x + 200 * std::cos(newAngle); double newY = this->robotPose.pos.y + 200 * std::sin(newAngle); usleep(200'000); this->go(newX, newY); - awaitRobotIdle(); + awaitRobotIdle();*/ } this->gameThread.~thread(); From 58343564f342e0cc8d7259e0cc8f1a7c22428e64 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 21:59:09 +0200 Subject: [PATCH 242/316] solar panel --- TCPServer.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 52b86bd..213ab18 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1225,7 +1225,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(700'000); + usleep(300'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: @@ -1236,7 +1236,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(700'000); + usleep(300'000); this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: @@ -1247,7 +1247,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(7); - usleep(700'000); + usleep(300'000); this->uncheckPanneau(7); break; default: @@ -1263,29 +1263,29 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { awaitRobotIdle(); this->checkPanneau(6); - usleep(700'000); + usleep(300'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: - this->go(2530, 1790); + this->go(2545, 1790); awaitRobotIdle(); this->rotate(PI); awaitRobotIdle(); this->checkPanneau(6); - usleep(700'000); + usleep(300'000); this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: - this->go(2325, 1790); + this->go(2350, 1790); awaitRobotIdle(); this->rotate(PI); awaitRobotIdle(); this->checkPanneau(6); - usleep(700'000); + usleep(300'000); this->uncheckPanneau(6); break; default: From 2000e95a0154a3c9739c719b153f28c0998f174d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 22:31:56 +0200 Subject: [PATCH 243/316] pos --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 213ab18..b40c99e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1584,14 +1584,14 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); if (team == BLUE) { if (sp == REMOVE_POT_J2) { - this->transit(230, 1100, 150); + this->transit(250, 1100, 150); awaitRobotIdle(); this->setMaxSpeed(); - this->go(190, 900); + this->go(210, 900); awaitRobotIdle(); - this->go(180, 400); + this->go(200, 400); awaitRobotIdle(); - this->go(230, 650); + this->go(250, 650); awaitRobotIdle(); } } else if (team == YELLOW) { From da8510930fffcc593181a29c7a5cff9128c9be86 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 23:42:15 +0200 Subject: [PATCH 244/316] speed --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b40c99e..bf28a6a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1515,7 +1515,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->baisserBras(); - this->setMaxSpeed(); + this->setSpeed(170); this->rotate(angle); awaitRobotIdle(); From 52deba1eb304ac1c58a531c4ecad1720f7f3a27c Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 23:42:34 +0200 Subject: [PATCH 245/316] speed --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index bf28a6a..5faa3ca 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1515,8 +1515,6 @@ void TCPServer::go3Plants(const StratPattern sp) { this->baisserBras(); - this->setSpeed(170); - this->rotate(angle); awaitRobotIdle(); From 9cd7a8186ac4492bb70de1cf011a9593af3d972f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Tue, 7 May 2024 23:44:13 +0200 Subject: [PATCH 246/316] do not need beacon --- TCPServer.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5faa3ca..f8c6553 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -244,7 +244,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); - this->broadcastMessage("strat;lidar;set beacon;1\n"); } else if (tokens[1] == "strat" && tokens[2] == "start") { From 2407f940e6c9b740cdf314a5f0f9d1eb1f6cc555 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 08:00:53 +0200 Subject: [PATCH 247/316] emergency --- TCPServer.cpp | 18 ++++++++++++++---- TCPServer.h | 3 +++ 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f8c6553..b87e440 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -522,6 +522,14 @@ void TCPServer::startGame() { case SLEEP_10S: usleep(10'000'000); break; + case ROTATE_0: + this->rotate(0); + awaitRobotIdle(); + break; + case ROTATE_270: + this->rotate(-PI/2); + awaitRobotIdle(); + break; } whereAmI++; } @@ -901,9 +909,13 @@ void TCPServer::handleEmergency(int distance, double angle) { this->stopEmergency = false; usleep(500'000); // ReSharper disable once CppDFAConstantConditions - if (this->stopEmergency) { + while (this->stopEmergency) { // TODO here go back by twenty centimeter // ReSharper disable once CppDFAUnreachableCode + + this->stopEmergency = false; + usleep(500'000); + /*double newAngle = this->robotPose.theta + angle; double newX = this->robotPose.pos.x + 200 * std::cos(newAngle); double newY = this->robotPose.pos.y + 200 * std::sin(newAngle); @@ -912,8 +924,6 @@ void TCPServer::handleEmergency(int distance, double angle) { awaitRobotIdle();*/ } - this->gameThread.~thread(); - this->gameStarted = false; this->gameThread = std::thread([this]() { this->startGame(); }); @@ -1556,7 +1566,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->rotate(angle); awaitRobotIdle(); - this->setMaxSpeed(); + this->setSpeed(160); this->go(this->robotPose.pos.x - (300 * direction), this->robotPose.pos.y); awaitRobotIdle(); diff --git a/TCPServer.h b/TCPServer.h index 795c8b6..8264ff6 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -60,6 +60,8 @@ enum StratPattern { SLEEP_1S, SLEEP_5S, SLEEP_10S, + ROTATE_0, + ROTATE_270, }; class TCPServer; // Forward declaration @@ -131,6 +133,7 @@ private: GET_LIDAR_POS, REMOVE_POT_J2, DROP_FLOWER_J2, + ROTATE_270, /*DROP_PURPLE_FLOWER, DROP_WHITE_FLOWER_J1,*/ From 7e8a8ded7c535ba55fe3201b2c93d914477ac3c5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 11:12:43 +0200 Subject: [PATCH 248/316] emergency --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index b87e440..1834952 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -906,8 +906,6 @@ void TCPServer::handleEmergency(int distance, double angle) { // TODO handle here the emergency like wait for 2 second and then if emergency is again on, that means the robot of the other team do not move, if that go back otherwise continue usleep(2'000'000); - this->stopEmergency = false; - usleep(500'000); // ReSharper disable once CppDFAConstantConditions while (this->stopEmergency) { // TODO here go back by twenty centimeter From 26f11c49ecdbce7f0d5c86e64a032c8bdfac1274 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 11:14:44 +0200 Subject: [PATCH 249/316] handle sigint and sigterm) --- main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/main.cpp b/main.cpp index e5b6443..51460a3 100644 --- a/main.cpp +++ b/main.cpp @@ -9,6 +9,7 @@ void signalHandler( int signum ) { int main(int argc, char* argv[]) { signal(SIGINT, signalHandler); + signal(SIGTERM, signalHandler); int port = 8080; if (argc >= 2) { From eb1b18e3a6412a0e6ad71b3c08e43b4075e5eb14 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 12:37:51 +0200 Subject: [PATCH 250/316] not started the game if already started --- TCPServer.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 1834952..f6de323 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -247,6 +247,10 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } else if (tokens[1] == "strat" && tokens[2] == "start") { + if (this->gameStarted) { + return; + } + this->broadcastMessage(message, clientSocket); this->gameStarted = true; From 1bf85817b990230bcbe6233cddbfd750b4085067 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 14:48:21 +0200 Subject: [PATCH 251/316] do not broadcast stop --- TCPServer.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f6de323..fa16923 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -127,9 +127,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->stopEmergency = true; this->gameThread.~thread(); - this->broadcastMessage("strat;arduino;clear;1"); - - this->broadcastMessage(message); + this->broadcastMessage("strat;arduino;clear;1\n"); std::vector args = TCPUtils::split(tokens[3], ","); From 2a24619a8a781fc0d65ed11797a32c1bc88a7e3b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 14:53:34 +0200 Subject: [PATCH 252/316] thread --- TCPServer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index fa16923..f74c1fb 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -126,9 +126,11 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (!gameStarted) return; this->stopEmergency = true; - this->gameThread.~thread(); + this->broadcastMessage("strat;arduino;clear;1\n"); + this->gameThread.~thread(); + std::vector args = TCPUtils::split(tokens[3], ","); if (!handleEmergencyFlag) { @@ -794,6 +796,9 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; + if (stopEmergency) { + std::terminate(); + } if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; From 4ba6313e7f7814905e4fad3e6eb3660416521313 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:00:48 +0200 Subject: [PATCH 253/316] clear --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f74c1fb..f3f28f7 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -125,10 +125,10 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (tokens[2] == "stop proximity") { if (!gameStarted) return; - this->stopEmergency = true; - this->broadcastMessage("strat;arduino;clear;1\n"); + this->stopEmergency = true; + this->gameThread.~thread(); std::vector args = TCPUtils::split(tokens[3], ","); From 25efdfa8bc24f95979c310f93bc4ea95d6d51604 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:01:16 +0200 Subject: [PATCH 254/316] alert proximity --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f3f28f7..33bd42e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -122,7 +122,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) std::cerr << "Invalid message format, token size : " << std::to_string(tokens.size()) << " from message : " << message << std::endl; return; } - if (tokens[2] == "stop proximity") { + if (TCPUtils::contains(tokens[2], "stop proximity")) { if (!gameStarted) return; this->broadcastMessage("strat;arduino;clear;1\n"); From 0dde5826954b94541c6a896ca65ca8fb80476e04 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:06:51 +0200 Subject: [PATCH 255/316] killed gameThread --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 33bd42e..9506da3 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -129,8 +129,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->stopEmergency = true; - this->gameThread.~thread(); - std::vector args = TCPUtils::split(tokens[3], ","); if (!handleEmergencyFlag) { @@ -929,6 +927,8 @@ void TCPServer::handleEmergency(int distance, double angle) { awaitRobotIdle();*/ } + this->gameThread.~thread(); + this->gameStarted = false; this->gameThread = std::thread([this]() { this->startGame(); }); From c6aa238ec4a2dfc877ed787e1abd3d7290ba6b28 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:07:47 +0200 Subject: [PATCH 256/316] try catch --- TCPServer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9506da3..474d356 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -927,7 +927,11 @@ void TCPServer::handleEmergency(int distance, double angle) { awaitRobotIdle();*/ } - this->gameThread.~thread(); + try { + this->gameThread.~thread(); + } catch (const std::exception& ex) { + std::cout << ex.what() << std::endl; + } this->gameStarted = false; From f6d80dd4faba815b9e9cf0c314a19ffb0749a663 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:09:45 +0200 Subject: [PATCH 257/316] thread --- TCPServer.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 474d356..b6bfe4b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -794,9 +794,6 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; - if (stopEmergency) { - std::terminate(); - } if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; From 32d959a513c6fb92ee253283eaf4cc6b376dfd0b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:13:38 +0200 Subject: [PATCH 258/316] clear --- TCPServer.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index b6bfe4b..c989e0b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -906,12 +906,15 @@ std::vector TCPServer::getNotFallenFlowers() const { void TCPServer::handleEmergency(int distance, double angle) { this->handleEmergencyFlag = true; + this->broadcastMessage("strat;arduino;clear;2\n"); + // TODO handle here the emergency like wait for 2 second and then if emergency is again on, that means the robot of the other team do not move, if that go back otherwise continue usleep(2'000'000); // ReSharper disable once CppDFAConstantConditions while (this->stopEmergency) { // TODO here go back by twenty centimeter // ReSharper disable once CppDFAUnreachableCode + this->broadcastMessage("strat;arduino;clear;3\n"); this->stopEmergency = false; usleep(500'000); @@ -923,6 +926,7 @@ void TCPServer::handleEmergency(int distance, double angle) { this->go(newX, newY); awaitRobotIdle();*/ } + this->broadcastMessage("strat;arduino;clear;4\n"); try { this->gameThread.~thread(); From d333e9db4a733cedcdf6ac3b182b43a9f1f35d08 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:14:15 +0200 Subject: [PATCH 259/316] sleep --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c989e0b..8270d0b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -917,7 +917,7 @@ void TCPServer::handleEmergency(int distance, double angle) { this->broadcastMessage("strat;arduino;clear;3\n"); this->stopEmergency = false; - usleep(500'000); + usleep(1'000'000); /*double newAngle = this->robotPose.theta + angle; double newX = this->robotPose.pos.x + 200 * std::cos(newAngle); From ccbf983023e8a60e94bc7f6c67313d6fc188ef5d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:18:47 +0200 Subject: [PATCH 260/316] sleep --- TCPServer.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8270d0b..6c6a0b6 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -123,8 +123,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) return; } if (TCPUtils::contains(tokens[2], "stop proximity")) { - if (!gameStarted) return; - this->broadcastMessage("strat;arduino;clear;1\n"); this->stopEmergency = true; @@ -917,7 +915,8 @@ void TCPServer::handleEmergency(int distance, double angle) { this->broadcastMessage("strat;arduino;clear;3\n"); this->stopEmergency = false; - usleep(1'000'000); + + std::this_thread::sleep_for(std::chrono::seconds(2)); /*double newAngle = this->robotPose.theta + angle; double newX = this->robotPose.pos.x + 200 * std::cos(newAngle); From 5e5956f5ba94f5d07be611204c15d017d6a9f7c4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 15:46:18 +0200 Subject: [PATCH 261/316] all ready --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 6c6a0b6..46cf29b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -435,6 +435,7 @@ void TCPServer::checkIfAllClientsReady() if (allReady) { + std::cout << "All clients are ready" << std::endl; this->broadcastMessage("strat;all;ready;1\n"); std::thread([this]() { askArduinoPos(); }).detach(); } From d98b1e7102e5063f6a19e6ce91ab5e0298496bd0 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 16:05:08 +0200 Subject: [PATCH 262/316] kill the thread --- TCPServer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 46cf29b..4733c70 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -793,6 +793,14 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; + if (stopEmergency) { + try { + std::terminate(); + } + catch (const std::exception& ex) { + std::cout << ex.what() << std::endl; + } + } if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; From 3bc0bc4bb93ac46d773a4330e519d3cd248f1417 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 16:07:17 +0200 Subject: [PATCH 263/316] kill the thread --- TCPServer.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 4733c70..74c9baf 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -936,12 +936,6 @@ void TCPServer::handleEmergency(int distance, double angle) { } this->broadcastMessage("strat;arduino;clear;4\n"); - try { - this->gameThread.~thread(); - } catch (const std::exception& ex) { - std::cout << ex.what() << std::endl; - } - this->gameStarted = false; this->gameThread = std::thread([this]() { this->startGame(); }); From ddd721d5644d31efa59ad98b24d4231227d8ef8f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 16:10:33 +0200 Subject: [PATCH 264/316] stopEmergency --- TCPServer.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 74c9baf..dde35ed 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -435,7 +435,6 @@ void TCPServer::checkIfAllClientsReady() if (allReady) { - std::cout << "All clients are ready" << std::endl; this->broadcastMessage("strat;all;ready;1\n"); std::thread([this]() { askArduinoPos(); }).detach(); } @@ -793,14 +792,6 @@ void TCPServer::awaitRobotIdle() { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); timeout++; - if (stopEmergency) { - try { - std::terminate(); - } - catch (const std::exception& ex) { - std::cout << ex.what() << std::endl; - } - } if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; @@ -936,6 +927,12 @@ void TCPServer::handleEmergency(int distance, double angle) { } this->broadcastMessage("strat;arduino;clear;4\n"); + try { + this->gameThread.~thread(); + } catch (const std::exception& ex) { + std::cout << ex.what() << std::endl; + } + this->gameStarted = false; this->gameThread = std::thread([this]() { this->startGame(); }); @@ -1686,39 +1683,47 @@ void TCPServer::checkpoint(const StratPattern sp) { template void TCPServer::go(X x, Y y) { + if (stopEmergency) return; 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; 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; 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; 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; this->broadcastMessage("strat;arduino;transit;" + std::to_string(static_cast(data[0])) + "," + std::to_string(static_cast(data[1])) + "," + std::to_string(endSpeed) + "\n"); } From bea252174a8a5f79f27183a975c66c903adefdf6 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 16:22:32 +0200 Subject: [PATCH 265/316] proximity --- TCPServer.cpp | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index dde35ed..7c132e6 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -443,6 +443,8 @@ void TCPServer::checkIfAllClientsReady() void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { + if (stopEmergency) return; + auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(82)) { this->goEnd(); @@ -1341,6 +1343,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { } } + if (stopEmergency) return; + this->setMaxSpeed(); this->go(whiteDropSetup); @@ -1367,6 +1371,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->sendPoint(3+1); } + if (stopEmergency) return; + usleep(500'000); this->closePince(0); @@ -1380,6 +1386,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->sendPoint(3+1); } + if (stopEmergency) return; + usleep(500'000); this->openPince(0); @@ -1432,6 +1440,8 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { return; } + if (stopEmergency) return; + this->setMaxSpeed(); this->go(dropPosition); @@ -1446,6 +1456,8 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->baisserBras(); + if (stopEmergency) return; + for (int i = 0; i < 3; i++) { this->openPince(i); } @@ -1468,6 +1480,8 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->closePince(i); } + if (stopEmergency) return; + if (!detectedPurple) { this->sendPoint(3); } @@ -1525,6 +1539,8 @@ void TCPServer::go3Plants(const StratPattern sp) { return; } + if (stopEmergency) return; + this->setMaxSpeed(); this->transit(plantPosition[0]-600, plantPosition[1], 170); @@ -1548,6 +1564,7 @@ void TCPServer::go3Plants(const StratPattern sp) { for (int i = 0; i < 5; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); + if (stopEmergency) return; } std::vector pinceCanTakeFLower = getNotFallenFlowers(); @@ -1566,6 +1583,8 @@ void TCPServer::go3Plants(const StratPattern sp) { usleep(500'000); + if (stopEmergency) return; + for (int i = 0; i < 3; i++) { this->closePince(i); pinceState[i] = pinceCanTakeFLower[i]; @@ -1768,34 +1787,42 @@ 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"); } @@ -1804,6 +1831,7 @@ void TCPServer::uncheckPanneau(int servo_moteur) { } void TCPServer::askLidarPosition() { + if (stopEmergency) return; this->broadcastMessage("start;lidar;get pos;1\n"); } From c4ef36cb08f5161333ccd160872cda25caf5a3c8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 16:26:45 +0200 Subject: [PATCH 266/316] proximity --- TCPServer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7c132e6..80d200d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -533,7 +533,8 @@ void TCPServer::startGame() { awaitRobotIdle(); break; } - whereAmI++; + + if (!stopEmergency) whereAmI++; } } From b5d3c18eb6d02f64ce2dee1eb442f9ea1ee3e7ff Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 16:59:02 +0200 Subject: [PATCH 267/316] lidarTimeout --- TCPServer.cpp | 6 ++++++ TCPServer.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 80d200d..86ac277 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -163,10 +163,14 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); } else if (tokens[0] == "lidar" && tokens[2] == "set pos") { + lidarGetPosTimeout++; std::vector args = TCPUtils::split(tokens[3], ","); // TODO replace angle with the real angle calculated by the lidar when working this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), /*std::stof(args[2]) / 100*/ this->robotPose.theta}; if (lidarCalculatePos.pos.x == -1 || lidarCalculatePos.pos.y == -1) { + if (lidarGetPosTimeout > 10) { + awaitForLidar = false; + } this->askLidarPosition(); } else { @@ -1647,6 +1651,8 @@ void TCPServer::removePot(StratPattern sp) { void TCPServer::getLidarPos() { + this->lidarGetPosTimeout = 0; + this->broadcastMessage("strat;arduino;clear;1\n"); usleep(1'000'000); diff --git a/TCPServer.h b/TCPServer.h index 8264ff6..2eb1062 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -175,6 +175,8 @@ private: int lidarSocket = -1; int arduinoSocket = -1; + int lidarGetPosTimeout = 0; + public: explicit TCPServer(int port); From b5ce066404ba11e9f0a1b0497a1115c671e80ff4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 17:47:39 +0200 Subject: [PATCH 268/316] change --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 86ac277..64e78f0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -123,6 +123,8 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) return; } if (TCPUtils::contains(tokens[2], "stop proximity")) { + if (!gameStarted) return; + this->broadcastMessage("strat;arduino;clear;1\n"); this->stopEmergency = true; From 6609115c2cbf51ce5217d4e72c72bbcda861b67b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 17:52:05 +0200 Subject: [PATCH 269/316] don't get lidar --- TCPServer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 2eb1062..3d95bfd 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -130,7 +130,7 @@ private: DROP_FLOWER_BASE_2,*/ TAKE_3_PLANT_BOTTOM_1, - GET_LIDAR_POS, + // GET_LIDAR_POS, REMOVE_POT_J2, DROP_FLOWER_J2, ROTATE_270, @@ -143,7 +143,7 @@ private: TAKE_FLOWER_TOP,*/ TAKE_3_PLANT_TOP_1, - GET_LIDAR_POS, + // GET_LIDAR_POS, DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, From afaf121b3495acd148b64b0880f3ce91e5314209 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 17:53:27 +0200 Subject: [PATCH 270/316] remove beacon --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 64e78f0..643faac 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -246,6 +246,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); + this->broadcastMessage("strat;lidar;set beacon;0\n"); } else if (tokens[1] == "strat" && tokens[2] == "start") { From e1a1c2659eb5c168d31ce85d5148af0a0d5899fc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 17:55:12 +0200 Subject: [PATCH 271/316] lidar --- TCPServer.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 643faac..64e78f0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -246,7 +246,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) } this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); - this->broadcastMessage("strat;lidar;set beacon;0\n"); } else if (tokens[1] == "strat" && tokens[2] == "start") { From fde7a7ddef75ffc990c489f397c61439abba4c7f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 18:01:14 +0200 Subject: [PATCH 272/316] lidar --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 64e78f0..e873f4e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -925,7 +925,7 @@ void TCPServer::handleEmergency(int distance, double angle) { this->stopEmergency = false; - std::this_thread::sleep_for(std::chrono::seconds(2)); + 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); From 1da2fe6d7c010e286a2bdb1053d24f93c49c0596 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 18:22:56 +0200 Subject: [PATCH 273/316] lidar pos --- TCPServer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.h b/TCPServer.h index 3d95bfd..2eb1062 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -130,7 +130,7 @@ private: DROP_FLOWER_BASE_2,*/ TAKE_3_PLANT_BOTTOM_1, - // GET_LIDAR_POS, + GET_LIDAR_POS, REMOVE_POT_J2, DROP_FLOWER_J2, ROTATE_270, @@ -143,7 +143,7 @@ private: TAKE_FLOWER_TOP,*/ TAKE_3_PLANT_TOP_1, - // GET_LIDAR_POS, + GET_LIDAR_POS, DROP_FLOWER_J1, /*DROP_PURPLE_FLOWER, From 87ad72c6c22c2a4ddcee3c51c493463ab2c73c38 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 19:18:26 +0200 Subject: [PATCH 274/316] game started --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index e873f4e..f1ef8e2 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -447,6 +447,7 @@ void TCPServer::checkIfAllClientsReady() } void TCPServer::startGame() { + gameStarted = true; for (int i = whereAmI; i < stratPatterns.size(); i++) { if (stopEmergency) return; From 96ea90958044afd9678a1eba573a3d8b8a8984da Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 21:24:32 +0200 Subject: [PATCH 275/316] avoid -1 distance --- TCPServer.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f1ef8e2..2fe0214 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -125,12 +125,14 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (TCPUtils::contains(tokens[2], "stop proximity")) { if (!gameStarted) return; + std::vector args = TCPUtils::split(tokens[3], ","); + + if (stoi(args[0]) == -1) return; + this->broadcastMessage("strat;arduino;clear;1\n"); this->stopEmergency = true; - std::vector args = TCPUtils::split(tokens[3], ","); - if (!handleEmergencyFlag) { std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1]) / 100); }).detach(); } From a74c2836a349bf7ac5768a5e7311fbcad8f15b8a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 21:42:20 +0200 Subject: [PATCH 276/316] solar pos --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2fe0214..07ebe35 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -213,7 +213,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) break; case 6: this->setTeam(YELLOW); - spawnPoint[0] = 1750; + spawnPoint[0] = 2750; spawnPoint[1] = 1800; spawnPoint[2] = PI; @@ -1259,7 +1259,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_2: - this->go(455, 1800); + this->go(460, 1800); awaitRobotIdle(); this->rotate(0); @@ -1270,7 +1270,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: - this->go(650, 1800); + this->go(665, 1800); awaitRobotIdle(); this->rotate(0); @@ -1286,7 +1286,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { } else if (team == YELLOW) { switch (sp) { case TURN_SOLAR_PANNEL_1: - this->go(2750, 1790); + this->go(2750, 1800); awaitRobotIdle(); this->rotate(PI); @@ -1297,7 +1297,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_2: - this->go(2545, 1790); + this->go(2540, 1800); awaitRobotIdle(); this->rotate(PI); @@ -1308,7 +1308,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: - this->go(2350, 1790); + this->go(2335, 1800); awaitRobotIdle(); this->rotate(PI); From 92dee387f5612e6417fc59c8677e31352083cdc8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 21:59:10 +0200 Subject: [PATCH 277/316] position --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 07ebe35..10d90f5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1553,7 +1553,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->transit(plantPosition[0]-600, plantPosition[1], 170); + this->transit(plantPosition[0]-(600*direction), plantPosition[1], 170); // this->go(plantPosition[0]-500, plantPosition[1]); awaitRobotIdle(); @@ -1562,7 +1562,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->rotate(angle); awaitRobotIdle(); - this->go(plantPosition[0]-400, plantPosition[1]); + this->go(plantPosition[0]-(400*direction), plantPosition[1]); awaitRobotIdle(); this->rotate(angle); From 5bfda393a58376d60147e93bd450f07de85431e4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Wed, 8 May 2024 22:28:47 +0200 Subject: [PATCH 278/316] team --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 10d90f5..50009e2 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -246,8 +246,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->setPosition(this->initRobotPose); usleep(100'000); } - - this->broadcastMessage("strat;lidar;set team;" + std::to_string(this->team) + "\n"); } else if (tokens[1] == "strat" && tokens[2] == "start") { From da7309a8de400f8946db15903eda9753f51b0036 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 00:47:49 +0200 Subject: [PATCH 279/316] speed --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 50009e2..8cd4ed4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1612,6 +1612,7 @@ void TCPServer::go3Plants(const StratPattern sp) { } usleep(200'000); + this->setSpeed(150); this->go(this->robotPose.pos.x + (200 * direction), this->robotPose.pos.y); awaitRobotIdle(); From 400e0101544f1d72a0889e00ff0cdf51afcb6a3f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 01:46:03 +0200 Subject: [PATCH 280/316] proximity --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8cd4ed4..5ad59ab 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -541,7 +541,7 @@ void TCPServer::startGame() { break; } - if (!stopEmergency) whereAmI++; + whereAmI++; } } From 644a375647188bb252b1fd4cdbf554f44b6d4070 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 02:31:58 +0200 Subject: [PATCH 281/316] 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); From 2ced2481de1b9aed350fa442483558bcd968f216 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 02:36:42 +0200 Subject: [PATCH 282/316] proximity --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 1a764cd..35f4903 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -804,6 +804,7 @@ void TCPServer::awaitRobotIdle() { timeout++; if (stopEmergency) { while (stopEmergency) { + stopEmergency = false; usleep(100'000); } this->broadcastMessage(lastArduinoCommand); From e6fab9e30f4b71757efb849b268e3abd66a8541b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 02:41:32 +0200 Subject: [PATCH 283/316] proximity --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 35f4903..d4d0aa1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -805,7 +805,7 @@ void TCPServer::awaitRobotIdle() { if (stopEmergency) { while (stopEmergency) { stopEmergency = false; - usleep(100'000); + usleep(300'000); } this->broadcastMessage(lastArduinoCommand); awaitRobotIdle(); From 0db17c273157382b3995691ee928a00ece9a768b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 02:53:17 +0200 Subject: [PATCH 284/316] pince end --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d4d0aa1..e4f586e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1009,8 +1009,10 @@ void TCPServer::goEnd() { this->rotate(this->endRobotPose.theta); awaitRobotIdle(); + this->baisserBras(); + for (int i = 0 ; i < 3; i++) { - this->middlePince(i); + this->openPince(i); usleep(50'000); } this->sendPoint(10); From 16474282f8c324100cd99b72ede866f56b8bb23b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 03:19:11 +0200 Subject: [PATCH 285/316] stopEmergency --- TCPServer.cpp | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index e4f586e..522a2d0 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -450,8 +450,6 @@ void TCPServer::startGame() { gameStarted = true; for (int i = whereAmI; i < stratPatterns.size(); i++) { - if (stopEmergency) return; - auto time = std::chrono::system_clock::now(); if (time - gameStart > std::chrono::seconds(82)) { this->goEnd(); @@ -801,7 +799,6 @@ void TCPServer::awaitRobotIdle() { while (isRobotIdle < 2) { usleep(50'000); this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket); - timeout++; if (stopEmergency) { while (stopEmergency) { stopEmergency = false; @@ -810,6 +807,7 @@ void TCPServer::awaitRobotIdle() { this->broadcastMessage(lastArduinoCommand); awaitRobotIdle(); } + timeout++; if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); break; @@ -1361,8 +1359,6 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { } } - if (stopEmergency) return; - this->setMaxSpeed(); this->go(whiteDropSetup); @@ -1389,8 +1385,6 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->sendPoint(3+1); } - if (stopEmergency) return; - usleep(500'000); this->closePince(0); @@ -1404,8 +1398,6 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->sendPoint(3+1); } - if (stopEmergency) return; - usleep(500'000); this->openPince(0); @@ -1458,8 +1450,6 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { return; } - if (stopEmergency) return; - this->setMaxSpeed(); this->go(dropPosition); @@ -1474,8 +1464,6 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->baisserBras(); - if (stopEmergency) return; - for (int i = 0; i < 3; i++) { this->openPince(i); } @@ -1498,8 +1486,6 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->closePince(i); } - if (stopEmergency) return; - if (!detectedPurple) { this->sendPoint(3); } @@ -1557,8 +1543,6 @@ void TCPServer::go3Plants(const StratPattern sp) { return; } - if (stopEmergency) return; - this->setMaxSpeed(); this->transit(plantPosition[0]-(600*direction), plantPosition[1], 170); @@ -1582,7 +1566,6 @@ void TCPServer::go3Plants(const StratPattern sp) { for (int i = 0; i < 5; i++) { this->broadcastMessage("strat;aruco;get aruco;1\n"); usleep(110'000); - if (stopEmergency) return; } std::vector pinceCanTakeFLower = getNotFallenFlowers(); @@ -1601,8 +1584,6 @@ void TCPServer::go3Plants(const StratPattern sp) { usleep(500'000); - if (stopEmergency) return; - for (int i = 0; i < 3; i++) { this->closePince(i); pinceState[i] = pinceCanTakeFLower[i]; From 4389e46d8a3867b7f5a1a60b45056f256524a14e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 10:37:59 +0200 Subject: [PATCH 286/316] solarPanelPos --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 522a2d0..add447b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1276,7 +1276,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(7); break; case TURN_SOLAR_PANNEL_3: - this->go(665, 1800); + this->go(690, 1800); awaitRobotIdle(); this->rotate(0); @@ -1314,7 +1314,7 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { this->uncheckPanneau(6); break; case TURN_SOLAR_PANNEL_3: - this->go(2335, 1800); + this->go(2310, 1800); awaitRobotIdle(); this->rotate(PI); From 7368baa3108ec321ac8dc477bf5165c9173371ae Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 12:04:24 +0200 Subject: [PATCH 287/316] speed --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index add447b..d7708ec 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1551,6 +1551,8 @@ void TCPServer::go3Plants(const StratPattern sp) { this->baisserBras(); + this->setMaxSpeed(); + this->rotate(angle); awaitRobotIdle(); From b3588cc33d52ad82fbeac54562cdfea398e7ee51 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 12:06:54 +0200 Subject: [PATCH 288/316] handle the fact that the lidar don't find balise --- TCPServer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index d7708ec..652ac5a 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -175,6 +175,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) if (lidarGetPosTimeout > 10) { awaitForLidar = false; } + lidarGetPosTimeout++; this->askLidarPosition(); } else { From 6379466ec052300be178c674f2d0a175b3cadc8a Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 12:16:01 +0200 Subject: [PATCH 289/316] lidar --- TCPServer.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 652ac5a..16c9bfd 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -167,7 +167,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket) this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket); } else if (tokens[0] == "lidar" && tokens[2] == "set pos") { - lidarGetPosTimeout++; std::vector args = TCPUtils::split(tokens[3], ","); // TODO replace angle with the real angle calculated by the lidar when working this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), /*std::stof(args[2]) / 100*/ this->robotPose.theta}; From 704c530bed7c4836fd3fc6e04bb8e1518454a7c0 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 14:20:02 +0200 Subject: [PATCH 290/316] timeout liar --- TCPServer.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 16c9bfd..200dd32 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1662,10 +1662,16 @@ void TCPServer::getLidarPos() { this->askLidarPosition(); + int timeout = 0; + // ReSharper disable once CppDFAConstantConditions // ReSharper disable once CppDFAEndlessLoop while (awaitForLidar) { usleep(50'000); + timeout++; + if (timeout > 100) { + break; + } } // ReSharper disable once CppDFAUnreachableCode From 96964c9c664a1b6f94157351a6af93986ba47da2 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 15:05:47 +0200 Subject: [PATCH 291/316] send point --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 200dd32..c0dd19d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1395,9 +1395,11 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { if (pinceState[1] != NONE) { this->fullyOpenPince(1); pinceState[1] = NONE; - this->sendPoint(3+1); } + this->sendPoint(3+1); + this->sendPoint(3+1); + usleep(500'000); this->openPince(0); From 400e82c2db9f912b3e46ee7a38447d142cbab92d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 9 May 2024 15:06:38 +0200 Subject: [PATCH 292/316] send point --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c0dd19d..c6c9b52 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1377,12 +1377,10 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { if (pinceState[0] != NONE) { this->fullyOpenPince(0); pinceState[0] = NONE; - this->sendPoint(3+1); } if (pinceState[2] != NONE) { this->fullyOpenPince(2); pinceState[2] = NONE; - this->sendPoint(3+1); } usleep(500'000); From 9afa0bbf7d23761309c46eabb5beb37ba4ff5088 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 00:56:30 +0200 Subject: [PATCH 293/316] =?UTF-8?q?jardini=C3=A8re?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- TCPServer.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c6c9b52..d40f6e8 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1339,22 +1339,22 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { double angle = PI / 2; if (team == BLUE) { if (sp == DROP_FLOWER_J1) { - whiteDropSetup = std::array{762, 300}; - whiteDropPosition = std::array{762, 0}; + whiteDropSetup = std::array{755, 300}; + whiteDropPosition = std::array{755, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - whiteDropSetup = std::array{300, 607}; - whiteDropPosition = std::array{0, 607}; + whiteDropSetup = std::array{300, 590}; + whiteDropPosition = std::array{0, 590}; angle = -PI; } } else if (team == YELLOW) { if (sp == DROP_FLOWER_J1) { - whiteDropSetup = std::array{2237, 300}; - whiteDropPosition = std::array{2237, 0}; + whiteDropSetup = std::array{2245, 300}; + whiteDropPosition = std::array{2245, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - whiteDropSetup = std::array{1700, 607}; - whiteDropPosition = std::array{0, 607}; + whiteDropSetup = std::array{2700, 590}; + whiteDropPosition = std::array{0, 590}; angle = 0; } } @@ -1556,7 +1556,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->rotate(angle); awaitRobotIdle(); - this->go(plantPosition[0]-(400*direction), plantPosition[1]); + this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150); awaitRobotIdle(); this->rotate(angle); From 01ca2cf5a3b94f0c87196517e572dcb8423efd6d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 01:05:04 +0200 Subject: [PATCH 294/316] remove pot --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index d40f6e8..2530e0b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1633,14 +1633,14 @@ void TCPServer::removePot(StratPattern sp) { } } else if (team == YELLOW) { if (sp == REMOVE_POT_J2) { - this->transit(2770, 1100, 150); + this->transit(2750, 1100, 150); awaitRobotIdle(); this->setMaxSpeed(); - this->go(2810, 900); + this->go(2790, 900); awaitRobotIdle(); - this->go(2810, 400); + this->go(2800, 400); awaitRobotIdle(); - this->go(2770, 650); + this->go(2750, 650); awaitRobotIdle(); } } From 14b6372e7a1375181ad91123b5f15526846e5ad5 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 01:19:30 +0200 Subject: [PATCH 295/316] drop jardiniere --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 2530e0b..56fee69 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1343,8 +1343,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { whiteDropPosition = std::array{755, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - whiteDropSetup = std::array{300, 590}; - whiteDropPosition = std::array{0, 590}; + whiteDropSetup = std::array{300, 600}; + whiteDropPosition = std::array{0, 600}; angle = -PI; } } else if (team == YELLOW) { @@ -1353,8 +1353,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { whiteDropPosition = std::array{2245, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { - whiteDropSetup = std::array{2700, 590}; - whiteDropPosition = std::array{0, 590}; + whiteDropSetup = std::array{2700, 600}; + whiteDropPosition = std::array{3000, 600}; angle = 0; } } From e1dd5f363a6d4ad666ca78d75387d9213119ea0d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 01:24:20 +0200 Subject: [PATCH 296/316] drop jardiniere --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 56fee69..865a8e1 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1349,8 +1349,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { } } else if (team == YELLOW) { if (sp == DROP_FLOWER_J1) { - whiteDropSetup = std::array{2245, 300}; - whiteDropPosition = std::array{2245, 0}; + whiteDropSetup = std::array{2230, 300}; + whiteDropPosition = std::array{2230, 0}; angle = PI / 2; } else if (sp == DROP_FLOWER_J2) { whiteDropSetup = std::array{2700, 600}; From bd11b213bbe13f974fbbfb257fc95e6d45f4419b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 01:38:36 +0200 Subject: [PATCH 297/316] update --- TCPServer.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 865a8e1..310858d 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1361,7 +1361,7 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->setMaxSpeed(); - this->go(whiteDropSetup); + this->transit(whiteDropSetup, 170); awaitRobotIdle(); this->rotate(angle); @@ -1597,7 +1597,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setSpeed(160); - this->go(this->robotPose.pos.x - (300 * direction), this->robotPose.pos.y); + this->go(this->robotPose.pos.x - (200 * direction), this->robotPose.pos.y); awaitRobotIdle(); for (int i = 0; i < 3; i++) { @@ -1606,7 +1606,7 @@ void TCPServer::go3Plants(const StratPattern sp) { usleep(200'000); this->setSpeed(150); - this->go(this->robotPose.pos.x + (200 * direction), this->robotPose.pos.y); + this->go(this->robotPose.pos.x + (75 * direction), this->robotPose.pos.y); awaitRobotIdle(); for (int i = 0; i < 3; i++) { @@ -1636,9 +1636,9 @@ void TCPServer::removePot(StratPattern sp) { this->transit(2750, 1100, 150); awaitRobotIdle(); this->setMaxSpeed(); - this->go(2790, 900); + this->go(2780, 900); awaitRobotIdle(); - this->go(2800, 400); + this->go(2790, 400); awaitRobotIdle(); this->go(2750, 650); awaitRobotIdle(); @@ -1688,6 +1688,8 @@ void TCPServer::checkpoint(const StratPattern sp) { this->awaitRobotIdle(); break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: + this->go(800, 1800); + awaitRobotIdle(); this->go(500, 1700); usleep(500'000); break; @@ -1701,6 +1703,8 @@ void TCPServer::checkpoint(const StratPattern sp) { this->awaitRobotIdle(); break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: + this->go(2200, 1800); + awaitRobotIdle(); this->go(2500, 1700); usleep(500'000); break; From b3fe74a2a4bfc9b210d5d8a507bb5bf3c194e162 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 01:40:59 +0200 Subject: [PATCH 298/316] timeout --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 310858d..857d544 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -451,7 +451,7 @@ void TCPServer::startGame() { for (int i = whereAmI; i < stratPatterns.size(); i++) { auto time = std::chrono::system_clock::now(); - if (time - gameStart > std::chrono::seconds(82)) { + if (time - gameStart > std::chrono::seconds(87)) { this->goEnd(); return; } From ae14d4b7ee975cec9723f4039cb88ab232ea77d8 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 02:02:43 +0200 Subject: [PATCH 299/316] awaitRobotIdle --- TCPServer.cpp | 147 ++++++++++++++++++++++++++------------------------ TCPServer.h | 2 +- 2 files changed, 78 insertions(+), 71 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 857d544..f7d69b5 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -531,11 +531,11 @@ void TCPServer::startGame() { break; case ROTATE_0: this->rotate(0); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; break; case ROTATE_270: this->rotate(-PI/2); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; break; } @@ -589,7 +589,7 @@ void TCPServer::startGameTest() { // pi/4 this->broadcastMessage("strat;arduino;angle;314\n"); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; // ReSharper disable once CppDFAUnreachableCode @@ -631,7 +631,7 @@ void TCPServer::startGameTest() { } this->broadcastMessage("strat;arduino;angle;157\n"); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; // this->broadcastMessage("strat;servo_moteur;baisser bras;1\n"); @@ -677,10 +677,10 @@ void TCPServer::startGameTest() { std::string toSend = "strat;arduino;go;762,300\n"; this->broadcastMessage(toSend); usleep(200'000); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->broadcastMessage("strat;arduino;angle;157\n"); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->broadcastMessage("strat;arduino;speed;150\n"); this->broadcastMessage("strat;arduino;go;762,0\n"); @@ -702,11 +702,11 @@ void TCPServer::startGameTest() { 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); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; toSend = "strat;arduino;angle;" + std::to_string(static_cast(this->endRobotPose.theta * 100)) + "\n"; this->broadcastMessage(toSend); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; // toSend = "start;arduino;angle;" + std::to_string(this->endRobotPose.theta * 100) + "\n"; // this->broadcastMessage(toSend); @@ -756,14 +756,14 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { this->setSpeed(200); this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX; double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY; this->transit(robotPosForPotX, robotPosForPotY, 130); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->closePince(pince); usleep(500'000); @@ -790,7 +790,7 @@ void TCPServer::askArduinoPos() { } } -void TCPServer::awaitRobotIdle() { +int TCPServer::awaitRobotIdle() { isRobotIdle = 0; int timeout = 0; // ReSharper disable once CppDFAConstantConditions @@ -807,12 +807,19 @@ void TCPServer::awaitRobotIdle() { this->broadcastMessage(lastArduinoCommand); awaitRobotIdle(); } + if (gameStarted) { + auto time = std::chrono::system_clock::now(); + if (time - gameStart > std::chrono::seconds(87)) { + return -1; + } + } timeout++; if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); - break; + return -2; } } + return 0; } void TCPServer::handleArucoTag(const ArucoTag &tag) { @@ -939,7 +946,7 @@ void TCPServer::handleEmergency(int distance, double angle) { double newY = this->robotPose.pos.y + 200 * std::sin(newAngle); usleep(200'000); this->go(newX, newY); - awaitRobotIdle();*/ + if (awaitRobotIdle() < 0) return;*/ /*} this->broadcastMessage("strat;arduino;clear;4\n"); @@ -998,14 +1005,14 @@ void TCPServer::goEnd() { for (const auto& checkpoint : checkponts) { this->go(checkpoint); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; } this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setSpeed(180); this->rotate(this->endRobotPose.theta); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->baisserBras(); @@ -1023,34 +1030,34 @@ void TCPServer::findAndGoFlower(const StratPattern sp) { if (team == BLUE) { if (sp == TAKE_FLOWER_TOP) { this->go(500, 700); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(0); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(500, 1300); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(0); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; } else { return; } } else if (team == YELLOW) { if (sp == TAKE_FLOWER_TOP) { this->go(1500, 700); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(-PI); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; } else if (sp == TAKE_FLOWER_BOTTOM) { this->go(1500, 1300); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(-PI); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; } else { return; } @@ -1123,12 +1130,12 @@ void TCPServer::dropPurpleFlowers() { if (!pinceHavePurpleFlower.empty()) { this->go(purpleDropPosition); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setSpeed(150); this->rotate(PI / 2); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->baisserBras(); @@ -1137,10 +1144,10 @@ void TCPServer::dropPurpleFlowers() { usleep(200'000); this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; pinceState[toDrop] = NONE; this->closePince(toDrop); @@ -1205,11 +1212,11 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) { this->setSpeed(200); this->go(whiteDropSetup); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(angle); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->leverBras(); usleep(500'000); @@ -1240,7 +1247,7 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) { this->setSpeed(200); this->go(whiteDropSetup); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; for (int i = 0; i < 3; i++) { this->closePince(i); @@ -1255,10 +1262,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { switch (sp) { case TURN_SOLAR_PANNEL_1: this->go(250, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(0); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->checkPanneau(7); usleep(300'000); @@ -1266,10 +1273,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { break; case TURN_SOLAR_PANNEL_2: this->go(460, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(0); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->checkPanneau(7); usleep(300'000); @@ -1277,10 +1284,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { break; case TURN_SOLAR_PANNEL_3: this->go(690, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(0); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->checkPanneau(7); usleep(300'000); @@ -1293,10 +1300,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { switch (sp) { case TURN_SOLAR_PANNEL_1: this->go(2750, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(PI); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->checkPanneau(6); usleep(300'000); @@ -1304,10 +1311,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { break; case TURN_SOLAR_PANNEL_2: this->go(2540, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(PI); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->checkPanneau(6); usleep(300'000); @@ -1315,10 +1322,10 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) { break; case TURN_SOLAR_PANNEL_3: this->go(2310, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(PI); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->checkPanneau(6); usleep(300'000); @@ -1362,10 +1369,10 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->setMaxSpeed(); this->transit(whiteDropSetup, 170); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(angle); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->leverBras(); @@ -1407,7 +1414,7 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->setMaxSpeed(); this->go(whiteDropSetup); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->transportBras(); } @@ -1453,12 +1460,12 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->setMaxSpeed(); this->go(dropPosition); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setSpeed(170); this->rotate(angle); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setMaxSpeed(); @@ -1469,10 +1476,10 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { } this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; bool detectedPurple = false; @@ -1547,20 +1554,20 @@ void TCPServer::go3Plants(const StratPattern sp) { this->transit(plantPosition[0]-(600*direction), plantPosition[1], 170); // this->go(plantPosition[0]-500, plantPosition[1]); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->baisserBras(); this->setMaxSpeed(); this->rotate(angle); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->rotate(angle); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setMaxSpeed(); @@ -1580,7 +1587,7 @@ void TCPServer::go3Plants(const StratPattern sp) { usleep(200'000); this->transit(plantPosition[0], this->robotPose.pos.y, 130); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setMaxSpeed(); @@ -1593,12 +1600,12 @@ void TCPServer::go3Plants(const StratPattern sp) { usleep(500'000); this->rotate(angle); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setSpeed(160); this->go(this->robotPose.pos.x - (200 * direction), this->robotPose.pos.y); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; for (int i = 0; i < 3; i++) { this->openPince(i); @@ -1607,7 +1614,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setSpeed(150); this->go(this->robotPose.pos.x + (75 * direction), this->robotPose.pos.y); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; for (int i = 0; i < 3; i++) { this->closePince(i); @@ -1622,26 +1629,26 @@ void TCPServer::removePot(StratPattern sp) { if (team == BLUE) { if (sp == REMOVE_POT_J2) { this->transit(250, 1100, 150); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setMaxSpeed(); this->go(210, 900); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(200, 400); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(250, 650); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; } } else if (team == YELLOW) { if (sp == REMOVE_POT_J2) { this->transit(2750, 1100, 150); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->setMaxSpeed(); this->go(2780, 900); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(2790, 400); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(2750, 650); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; } } } @@ -1685,11 +1692,11 @@ void TCPServer::checkpoint(const StratPattern sp) { switch (sp) { case CHECKPOINT_MIDDLE: this->go(500, 1500); - this->awaitRobotIdle(); + this->if (awaitRobotIdle() < 0) return; break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(800, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(500, 1700); usleep(500'000); break; @@ -1700,11 +1707,11 @@ void TCPServer::checkpoint(const StratPattern sp) { switch (sp) { case CHECKPOINT_MIDDLE: this->go(2500, 1500); - this->awaitRobotIdle(); + this->if (awaitRobotIdle() < 0) return; break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(2200, 1800); - awaitRobotIdle(); + if (awaitRobotIdle() < 0) return; this->go(2500, 1700); usleep(500'000); break; diff --git a/TCPServer.h b/TCPServer.h index 1d9b6a7..2495415 100644 --- a/TCPServer.h +++ b/TCPServer.h @@ -216,7 +216,7 @@ public: [[nodiscard]] bool shouldStop() const; - void awaitRobotIdle(); + int awaitRobotIdle(); void handleArucoTag(const ArucoTag &tag); From 15d3928b713b67ac0d4ad8a02b72fcbe40e617ce Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 02:03:26 +0200 Subject: [PATCH 300/316] awaitRobotIdle --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index f7d69b5..41e92a4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1692,7 +1692,7 @@ void TCPServer::checkpoint(const StratPattern sp) { switch (sp) { case CHECKPOINT_MIDDLE: this->go(500, 1500); - this->if (awaitRobotIdle() < 0) return; + if (awaitRobotIdle() < 0) return; break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(800, 1800); @@ -1707,7 +1707,7 @@ void TCPServer::checkpoint(const StratPattern sp) { switch (sp) { case CHECKPOINT_MIDDLE: this->go(2500, 1500); - this->if (awaitRobotIdle() < 0) return; + if (awaitRobotIdle() < 0) return; break; case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER: this->go(2200, 1800); From 6f66278e2b880dfa3e07f432800443ec6328e2a0 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 02:24:08 +0200 Subject: [PATCH 301/316] awaitRobotIdle --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 41e92a4..5f48675 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -816,7 +816,7 @@ int TCPServer::awaitRobotIdle() { timeout++; if (timeout > 80) { this->broadcastMessage("strat;arduino;clear;1"); - return -2; + return 1; } } return 0; From 482ebd7603234d92afc29205ea43652a3d0d77dc Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 02:44:56 +0200 Subject: [PATCH 302/316] remove pot --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5f48675..72a28ab 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1645,7 +1645,7 @@ void TCPServer::removePot(StratPattern sp) { this->setMaxSpeed(); this->go(2780, 900); if (awaitRobotIdle() < 0) return; - this->go(2790, 400); + this->go(2780, 400); if (awaitRobotIdle() < 0) return; this->go(2750, 650); if (awaitRobotIdle() < 0) return; From b0882f7d8a09230bd0e00e61f7573f9f37bb7bc4 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 02:52:33 +0200 Subject: [PATCH 303/316] max speed --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 72a28ab..7dba371 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1371,6 +1371,8 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { this->transit(whiteDropSetup, 170); if (awaitRobotIdle() < 0) return; + this->setMaxSpeed(); + this->rotate(angle); if (awaitRobotIdle() < 0) return; @@ -1552,7 +1554,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->transit(plantPosition[0]-(600*direction), plantPosition[1], 170); + this->transit(plantPosition[0]-(600*direction), plantPosition[1], 150); // this->go(plantPosition[0]-500, plantPosition[1]); if (awaitRobotIdle() < 0) return; From eedd699d8e7dc8b23619793792ccda134b445cbe Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 03:00:40 +0200 Subject: [PATCH 304/316] speed --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7dba371..9083d1c 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1464,8 +1464,6 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->go(dropPosition); if (awaitRobotIdle() < 0) return; - this->setSpeed(170); - this->rotate(angle); if (awaitRobotIdle() < 0) return; From 6db7815c3a65194ed175778caca7983802f86594 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 03:04:12 +0200 Subject: [PATCH 305/316] speed --- TCPServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 9083d1c..382aa4e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1566,6 +1566,8 @@ void TCPServer::go3Plants(const StratPattern sp) { this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150); if (awaitRobotIdle() < 0) return; + this->setSpeed(180); + this->rotate(angle); if (awaitRobotIdle() < 0) return; From 42b7845456d7f98a8d5402ca3b5eec65ebef947b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 03:07:08 +0200 Subject: [PATCH 306/316] speed --- TCPServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 382aa4e..a4f5461 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1566,7 +1566,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150); if (awaitRobotIdle() < 0) return; - this->setSpeed(180); + this->setSpeed(165); this->rotate(angle); if (awaitRobotIdle() < 0) return; From 8bd31317ce749e379897a76a125020d9b3ead284 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 03:07:36 +0200 Subject: [PATCH 307/316] speed --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index a4f5461..c250b2e 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1558,11 +1558,11 @@ void TCPServer::go3Plants(const StratPattern sp) { this->baisserBras(); - this->setMaxSpeed(); - this->rotate(angle); if (awaitRobotIdle() < 0) return; + this->setMaxSpeed(); + this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150); if (awaitRobotIdle() < 0) return; From d7880517e73de390c3f7f9d55fffbfcb1f601595 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 03:11:26 +0200 Subject: [PATCH 308/316] speed --- TCPServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index c250b2e..7e38c15 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1558,6 +1558,8 @@ void TCPServer::go3Plants(const StratPattern sp) { this->baisserBras(); + this->setSpeed(170); + this->rotate(angle); if (awaitRobotIdle() < 0) return; @@ -1566,7 +1568,7 @@ void TCPServer::go3Plants(const StratPattern sp) { this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150); if (awaitRobotIdle() < 0) return; - this->setSpeed(165); + this->setSpeed(170); this->rotate(angle); if (awaitRobotIdle() < 0) return; From cc91d9c978b6c2411c71a4542bb6e1c8e39d8bcb Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 10:26:30 +0200 Subject: [PATCH 309/316] remove aruco --- TCPServer.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 7e38c15..112d0b4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1575,18 +1575,16 @@ void TCPServer::go3Plants(const StratPattern sp) { this->setMaxSpeed(); - this->arucoTags.clear(); - for (int i = 0; i < 5; i++) { - this->broadcastMessage("strat;aruco;get aruco;1\n"); - usleep(110'000); - } + // this->arucoTags.clear(); + // for (int i = 0; i < 5; i++) { + // this->broadcastMessage("strat;aruco;get aruco;1\n"); + // usleep(110'000); + // } - std::vector pinceCanTakeFLower = getNotFallenFlowers(); + // std::vector pinceCanTakeFLower = getNotFallenFlowers(); for (int i = 0; i < 3; i++) { - if (pinceCanTakeFLower[i] != NONE) { - this->openPince(i); - } + this->openPince(i); } usleep(200'000); @@ -1599,7 +1597,7 @@ void TCPServer::go3Plants(const StratPattern sp) { for (int i = 0; i < 3; i++) { this->closePince(i); - pinceState[i] = pinceCanTakeFLower[i]; + pinceState[i] = FLOWER; } usleep(500'000); From 5bfc49a4951a2f0b3104bc9d38ded17d4944005b Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 10:31:38 +0200 Subject: [PATCH 310/316] remove aruco --- TCPServer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 112d0b4..857fe01 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -72,10 +72,10 @@ TCPServer::TCPServer(int port) : team(TEST) std::cout << "Server started on port " << port << std::endl; - clients.reserve(6); + clients.reserve(5); clients.emplace_back("tirette"); - clients.emplace_back("aruco"); + // clients.emplace_back("aruco"); clients.emplace_back("ihm"); clients.emplace_back("lidar"); clients.emplace_back("arduino"); From d6a008aac727975418e76fefc7238eab3cb50d5e Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 13:49:31 +0200 Subject: [PATCH 311/316] remove aruco --- TCPServer.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 857fe01..95b2124 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -486,6 +486,7 @@ void TCPServer::startGame() { break; case GET_LIDAR_POS: getLidarPos(); + usleep(200'000); break; case CHECKPOINT_MIDDLE: checkpoint(CHECKPOINT_MIDDLE); @@ -1424,17 +1425,20 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { void TCPServer::dropBaseFlowers(StratPattern sp) { std::array dropPosition{}; double angle; + double Positioningangle; float distance; if (team == BLUE) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {300, 400}; angle = PI / 2; + Positioningangle = 0; distance = 150; } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {300, 1600}; angle = -PI / 2; + Positioningangle = 0; distance = -150; } else { @@ -1445,11 +1449,13 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {2700, 400}; angle = PI / 2; + Positioningangle = PI; distance = 150; } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {2700, 1600}; angle = -PI / 2; + Positioningangle = PI; distance = -150; } else { return; @@ -1461,6 +1467,9 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->setMaxSpeed(); + this->rotate(Positioningangle); + if (awaitRobotIdle() < 0) return; + this->go(dropPosition); if (awaitRobotIdle() < 0) return; From f34b21556d2e1ebe150110a1fe572281ab13d3ca Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 13:56:07 +0200 Subject: [PATCH 312/316] patch --- TCPServer.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 95b2124..599b1bc 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1425,20 +1425,17 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { void TCPServer::dropBaseFlowers(StratPattern sp) { std::array dropPosition{}; double angle; - double Positioningangle; float distance; if (team == BLUE) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {300, 400}; angle = PI / 2; - Positioningangle = 0; distance = 150; } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {300, 1600}; angle = -PI / 2; - Positioningangle = 0; distance = -150; } else { @@ -1449,13 +1446,14 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {2700, 400}; angle = PI / 2; - Positioningangle = PI; distance = 150; + + this->rotate(0); + awaitRobotIdle(); } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {2700, 1600}; angle = -PI / 2; - Positioningangle = PI; distance = -150; } else { return; @@ -1467,9 +1465,6 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->setMaxSpeed(); - this->rotate(Positioningangle); - if (awaitRobotIdle() < 0) return; - this->go(dropPosition); if (awaitRobotIdle() < 0) return; From 5dd7360983bed86778368bbbcd7e1a33e5fcbf38 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Fri, 10 May 2024 19:25:21 +0200 Subject: [PATCH 313/316] angleStart --- TCPServer.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 599b1bc..876b551 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1425,17 +1425,20 @@ void TCPServer::dropJardiniereFlowers(const StratPattern sp) { void TCPServer::dropBaseFlowers(StratPattern sp) { std::array dropPosition{}; double angle; + double angleStart; float distance; if (team == BLUE) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {300, 400}; angle = PI / 2; + angleStart = PI; distance = 150; } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {300, 1600}; angle = -PI / 2; + angleStart = PI; distance = -150; } else { @@ -1446,14 +1449,13 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {2700, 400}; angle = PI / 2; + angleStart = 0; distance = 150; - - this->rotate(0); - awaitRobotIdle(); } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {2700, 1600}; angle = -PI / 2; + angleStart = 0; distance = -150; } else { return; @@ -1465,6 +1467,9 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->setMaxSpeed(); + this->rotate(angleStart); + if (awaitRobotIdle() < 0) return; + this->go(dropPosition); if (awaitRobotIdle() < 0) return; From f1365a2af4a1232156761c93ae18cc1bcce02930 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 11 May 2024 10:33:39 +0200 Subject: [PATCH 314/316] why that don't work --- TCPServer.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/TCPServer.cpp b/TCPServer.cpp index 876b551..8016049 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1470,6 +1470,11 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->rotate(angleStart); if (awaitRobotIdle() < 0) return; + this->transit(dropPosition, 170); + if (awaitRobotIdle() < 0) return; + + this->setMaxSpeed(); + this->go(dropPosition); if (awaitRobotIdle() < 0) return; From 28527ef1657775601ced21951d1020cce77df76f Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 11 May 2024 10:36:44 +0200 Subject: [PATCH 315/316] drop pos angle --- TCPServer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8016049..95da237 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1432,13 +1432,13 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {300, 400}; angle = PI / 2; - angleStart = PI; + angleStart = 3 * PI / 4; distance = 150; } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {300, 1600}; angle = -PI / 2; - angleStart = PI; + angleStart = -3 * PI / 4; distance = -150; } else { @@ -1449,13 +1449,13 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { if (sp == DROP_FLOWER_BASE_1) { dropPosition = {2700, 400}; angle = PI / 2; - angleStart = 0; + angleStart = PI / 4; distance = 150; } else if (sp == DROP_FLOWER_BASE_2) { dropPosition = {2700, 1600}; angle = -PI / 2; - angleStart = 0; + angleStart = -PI / 4; distance = -150; } else { return; From c0a976a5d6c3885935ca0844eca1d29212f9d973 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Sat, 11 May 2024 10:39:22 +0200 Subject: [PATCH 316/316] remove speed --- TCPServer.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 95da237..df1c13b 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -1481,8 +1481,6 @@ void TCPServer::dropBaseFlowers(StratPattern sp) { this->rotate(angle); if (awaitRobotIdle() < 0) return; - this->setMaxSpeed(); - this->baisserBras(); for (int i = 0; i < 3; i++) {