From 7b699ef6733f63ad565d84840225a0bbcb762e30 Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 11 Apr 2024 15:17:12 +0200 Subject: [PATCH] aruco pos --- TCPServer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 5817a11..f237e33 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -364,12 +364,12 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) { this->broadcastMessage(toSend); double xPrime = arucoTagPos.pos.first[0] - 95; - double yPrime = arucoTagPos.pos.first[1]; + double yPrime = arucoTagPos.pos.first[1] - decalage; std::cout << "Aruco position1 " << xPrime << " " << yPrime << std::endl; - double posV200X = (xPrime * std::cos(theta) + (yPrime - decalage) * std::sin(theta)) + robotPosX; - double posV200Y = (-xPrime * std::sin(theta) + (yPrime - decalage) * std::cos(theta)) + robotPosY; + double posV200X = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX; + double posV200Y = (-xPrime * std::sin(theta) + yPrime * 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,10 +377,10 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) { this->broadcastMessage("strat;arduino;speed;150\n"); usleep(500'000); - xPrime += 95; + xPrime += 90; - double robotPosForPotX = (xPrime * std::cos(theta) + (yPrime - decalage) * std::sin(theta)) + robotPosX; - double robotPosForPotY = (-xPrime * std::sin(theta) + (yPrime - decalage) * std::cos(theta)) + robotPosY; + double robotPosForPotX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX; + double robotPosForPotY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY; std::cout << "Aruco position " << robotPosForPotX << " " << robotPosForPotY << std::endl;