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