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); }