From 9f7a2078a2fcf43b56bf5c65542055cd80ebfbee Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Mon, 15 Apr 2024 20:00:48 +0200
Subject: [PATCH] add test for the angle

---
 TCPServer.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index e6d8781..70ada9e 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -474,12 +474,12 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
     double xPrime = arucoTag.pos().first[0]/* - 5*/;
     double yPrime = arucoTag.pos().first[1] - decalage;
 
-    double testX = (xPrime * std::cos(theta) + yPrime * std::sin(theta)) + robotPosX;
-    double testY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta)) + robotPosY;
+    double testX = (xPrime * std::cos(theta) + yPrime * std::sin(theta));
+    double testY = (-xPrime * std::sin(theta) + yPrime * std::cos(theta));
 
     // calculate the angle to be in front of the tag
 
-    double thetaPrime = std::atan2(testX, testY);
+    double thetaPrime = std::atan2(testY, testX);
 
     toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate + thetaPrime) * 100)) + "\n";
     canMove = false;