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;