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

---
 TCPServer.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index ceeaa5f..154755c 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -480,16 +480,16 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
 
     switch (pince) {
         case 0:
-            decalage = -30;
-            rotate = 0.15;
+            decalage = 60;
+            rotate = -0.3;
             break;
         case 1:
             decalage = 0;
             rotate = 0;
             break;
         case 2:
-            decalage = 30;
-            rotate = -0.15;
+            decalage = -60;
+            rotate = 0.3;
             break;
         default:
             decalage = 0;
@@ -503,11 +503,11 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
     this->broadcastMessage(toSend);
 
     double xPrime = arucoTag.pos().first[0] + 30/* - 5*/ + (decalage * 0.2);
-    double yPrime = arucoTag.pos().first[1] - decalage;
+    double yPrime = arucoTag.pos().first[1] + decalage;
 
-    double thetaPrime = std::atan2(yPrime, xPrime);
+    double thetaPrime = std::atan2(-yPrime, xPrime);
 
-    toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>((this->robotPose.theta + rotate - thetaPrime) * 100)) + "\n";
+    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) {