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;