diff --git a/TCPServer.cpp b/TCPServer.cpp index 9682d25..4540d01 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -673,12 +673,12 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) { double yPrime = arucoTag.pos()[1]; double roll = arucoTag.rot()[1]; - auto centerPlantX = (20 * std::cos(roll)) + xPrime - 50; + auto centerPlantX = (20 * std::cos(roll)) + xPrime; auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage; double thetaPrime = std::atan2(centerPlantY, centerPlantX); - this->rotate(this->robotPose.theta + rotate - thetaPrime); + this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime); awaitRobotIdle(); double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX;