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 */