From b0f09eb80f8396ce57e17bcdd7278d42329f17c0 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Thu, 11 Apr 2024 15:03:07 +0200
Subject: [PATCH] aruco pos

---
 TCPServer.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 479cc22..380a1d0 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -368,8 +368,8 @@ void TCPServer::goToAruco(const ArucoTagPos &arucoTagPos, const int pince) {
 
     std::cout << "Aruco position1 " << xPrime << " " << yPrime << std::endl;
 
-    double posV200X = ((xPrime - 100) * std::cos(theta) + (yPrime - (decalage / 2)) * std::sin(theta)) + robotPosX;
-    double posV200Y = (-(xPrime - 100) * std::sin(theta) + (yPrime - (decalage / 2)) * std::cos(theta)) + robotPosY;
+    double posV200X = ((xPrime - 100) * std::cos(theta) + (yPrime - decalage) * std::sin(theta)) + robotPosX;
+    double posV200Y = (-(xPrime - 100) * std::sin(theta) + (yPrime - decalage) * std::cos(theta)) + robotPosY;
 
     toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(posV200X)) + "," + std::to_string(static_cast<int>(posV200Y)) + "\n";
     this->broadcastMessage(toSend);