From 947bcd2b722275686732d87bfbb90ebbde7d2c60 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Tue, 16 Apr 2024 13:31:59 +0200
Subject: [PATCH] angle

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

diff --git a/TCPServer.cpp b/TCPServer.cpp
index a1754d4..b64a76f 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -502,12 +502,15 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
 
     this->broadcastMessage(toSend);
 
-    double xPrime = arucoTag.pos().first[0] + 30/* - 5*/ + (decalage * 0.2);
+    double xPrime = arucoTag.pos().first[0];
     double yPrime = arucoTag.pos().first[1] + decalage;
 
-    double thetaPrime = std::atan2(-yPrime, xPrime);
+    xPrime *= 1.03;
+    yPrime *= 1.03;
 
-    toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate + thetaPrime) * 100)) + "\n";
+    double thetaPrime = std::atan2(yPrime, xPrime);
+
+    toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate - thetaPrime) * 100)) + "\n";
     this->broadcastMessage(toSend);
     isRobotMoving = 0;
     while (this->isRobotMoving < 3) {