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

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

diff --git a/TCPServer.cpp b/TCPServer.cpp
index a13fbc8..0df1acf 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -475,9 +475,9 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
     double yPrime = arucoTag.pos().first[1] - decalage;
 
     // calculate the angle to be in front of the tag
-    std::cout << "Tag pos relative : " << xPrime << " " << yPrime << std::endl;
+    std::cout << "Tag pos relative : " << xPrime << " " << yPrime << " " << this->robotPose.theta << std::endl;
 
-    double thetaPrime = std::atan2(xPrime, yPrime);
+    double thetaPrime = std::atan2(yPrime, xPrime);
 
     toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate + thetaPrime) * 100)) + "\n";
     canMove = false;