diff --git a/TCPServer.cpp b/TCPServer.cpp index 189c61c..806a0e4 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -335,10 +335,10 @@ void TCPServer::startGame() { } void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) { - float robotPosX = this->robotPose.pos.x; - float robotPosY = this->robotPose.pos.y; - float theta = this->robotPose.theta; - float decalage; + double robotPosX = this->robotPose.pos.x; + double robotPosY = this->robotPose.pos.y; + double theta = this->robotPose.theta; + double decalage; if (pince < 0 || pince > 2) { return; } @@ -366,10 +366,16 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) { double xPrime = arucoTagPos.pos.first[0] - 95; double yPrime = arucoTagPos.pos.first[1]; + double x10Percent = xPrime * 0.1; + double decalage10Percent = decalage * 0.1; + + xPrime -= x10Percent; + decalage -= decalage10Percent; + std::cout << "Aruco position1 " << xPrime << " " << yPrime << std::endl; - double posV200X = (xPrime * std::cos(theta) + (yPrime - (3 * decalage / 4)) * std::sin(theta)) + robotPosX; - double posV200Y = (-xPrime * std::sin(theta) + (yPrime - (3 * decalage / 4)) * std::cos(theta)) + robotPosY; + double posV200X = (xPrime * std::cos(theta) + (yPrime - decalage) * std::sin(theta)) + robotPosX; + double posV200Y = (-xPrime * std::sin(theta) + (yPrime - decalage) * std::cos(theta)) + robotPosY; toSend = "strat;arduino;go;" + std::to_string(static_cast(posV200X)) + "," + std::to_string(static_cast(posV200Y)) + "\n"; this->broadcastMessage(toSend); @@ -377,7 +383,8 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) { this->broadcastMessage("strat;arduino;speed;150\n"); usleep(500'000); - xPrime += 90; + xPrime += x10Percent; + decalage += decalage10Percent; double robotPosForPotX = (xPrime * std::cos(theta) + (yPrime - decalage) * std::sin(theta)) + robotPosX; double robotPosForPotY = (-xPrime * std::sin(theta) + (yPrime - decalage) * std::cos(theta)) + robotPosY;