From bb3b03173f44438f64b8a215f4446cb3d282f35d Mon Sep 17 00:00:00 2001 From: ackimixs Date: Thu, 11 Apr 2024 14:37:23 +0200 Subject: [PATCH] aruco pos --- TCPServer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TCPServer.cpp b/TCPServer.cpp index 8bd3669..c4fa909 100644 --- a/TCPServer.cpp +++ b/TCPServer.cpp @@ -361,12 +361,12 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) { this->broadcastMessage(toSend); double xPrime = arucoTagPos.pos.first[0] - 5; - double yPrime = arucoTagPos.pos.first[1] + decalage; + double yPrime = arucoTagPos.pos.first[1]; std::cout << "Aruco position1 " << xPrime << " " << yPrime << std::endl; - double posV200X = ((xPrime - 100) * std::cos(robotPose.theta) + yPrime * std::sin(robotPose.theta)) + this->robotPose.pos.x; - double posV200Y = (-(xPrime - 100) * std::sin(robotPose.theta) + yPrime * std::cos(robotPose.theta)) + this->robotPose.pos.y; + double posV200X = ((xPrime - 100) * std::cos(robotPose.theta) + (yPrime - (decalage / 2)) * std::sin(robotPose.theta)) + this->robotPose.pos.x; + double posV200Y = (-(xPrime - 100) * std::sin(robotPose.theta) + (yPrime - (decalage / 2)) * std::cos(robotPose.theta)) + this->robotPose.pos.y; toSend = "strat;arduino;go;" + std::to_string(static_cast(posV200X)) + "," + std::to_string(static_cast(posV200Y)) + "\n"; this->broadcastMessage(toSend); @@ -374,8 +374,8 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) { this->broadcastMessage("strat;arduino;speed;150\n"); usleep(500'000); - double robotPosForPotX = (xPrime * std::cos(robotPose.theta) + yPrime * std::sin(robotPose.theta)) + this->robotPose.pos.x; - double robotPosForPotY = (-xPrime * std::sin(robotPose.theta) + yPrime * std::cos(robotPose.theta)) + this->robotPose.pos.y; + double robotPosForPotX = (xPrime * std::cos(robotPose.theta) + (yPrime - decalage) * std::sin(robotPose.theta)) + this->robotPose.pos.x; + double robotPosForPotY = (-xPrime * std::sin(robotPose.theta) + (yPrime - decalage) * std::cos(robotPose.theta)) + this->robotPose.pos.y; std::cout << "Aruco position " << robotPosForPotX << " " << robotPosForPotY << std::endl;