From 7e4d1d76a9853148a8be2ae4e4e79688e134cb06 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Mon, 29 Apr 2024 22:45:43 +0200
Subject: [PATCH] change aruco finder

---
 TCPServer.cpp | 36 ++++++++++++++++++++++++++----------
 TCPServer.h   |  2 ++
 utils.cpp     |  4 ++++
 utils.h       |  3 +++
 4 files changed, 35 insertions(+), 10 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 35fc58c..e5811a0 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -429,7 +429,8 @@ void TCPServer::startGame() {
     for (int i = whereAmI; i < stratPatterns.size(); i++) {
 
         auto time = std::chrono::system_clock::now();
-        if (time - gameStart > std::chrono::seconds(85)) {
+        // TODO check for the bbest timing to return to the spawn
+        if (time - gameStart > std::chrono::seconds(80)) {
             this->goEnd();
             return;
         }
@@ -643,7 +644,6 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
     double robotPosY = this->robotPose.pos.y;
     double theta = this->robotPose.theta;
     double decalage;
-    double rotate;
     if (pince < 0 || pince > 2) {
         return;
     }
@@ -651,19 +651,15 @@ void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
     switch (pince) {
         case 0:
             decalage = 60;
-            rotate = -0.07;
             break;
         case 1:
             decalage = 0;
-            rotate = 0;
             break;
         case 2:
             decalage = -60;
-            rotate = 0.07;
             break;
         default:
             decalage = 0;
-            rotate = 0;
             break;
     }
 
@@ -767,6 +763,26 @@ std::optional<ArucoTag> TCPServer::getBiggestArucoTag(float borneMinX, float bor
     return found ? std::optional(biggestTag) : std::nullopt;
 }
 
+
+std::optional<ArucoTag> TCPServer::getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY) {
+    bool found = false;
+    ArucoTag mostCenteredTag = ArucoTag();
+    for (const auto & tag : arucoTags) {
+        if (tag.getNbFind() < 2) break;
+
+        if (!found) {
+            if (tag.pos()[0] > borneMinX && tag.pos()[0] < borneMaxX && tag.pos()[1] > borneMinY && tag.pos()[1] < borneMaxY) {
+                mostCenteredTag = tag;
+                found = true;
+            }
+        } else if (distanceToTag(tag) < distanceToTag(mostCenteredTag)) {
+            mostCenteredTag = tag;
+        }
+    }
+
+    return found ? std::optional(mostCenteredTag) : std::nullopt;
+}
+
 void TCPServer::handleEmergency(int distance, double angle) {
     this->handleEmergencyFlag = true;
 
@@ -871,13 +887,13 @@ void TCPServer::findAndGoFlower(StratPattern sp) {
 
     this->arucoTags.clear();
     this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 3; i++) {
-        usleep(400'000);
+    for (int i = 0; i < 4; i++) {
+        usleep(250'000);
         this->broadcastMessage("strat;aruco;get aruco;1\n");
     }
     usleep(100'000);
 
-    std::optional<ArucoTag> tag = getBiggestArucoTag(100, 800, -200, 200);
+    std::optional<ArucoTag> tag = getMostCenteredArucoTag(100, 800, -200, 200);
 
     if (tag.has_value()) {
         if (pinceState[1] == NONE) {
@@ -980,7 +996,7 @@ void TCPServer::dropFlowers() {
         this->setSpeed(130);
 
         this->go(whiteDropPosition[0], 0);
-        usleep(1'000'000);
+        usleep(2'000'000);
 
         for (int i = 0; i < 3; i++) {
             if (pinceState[i] == WHITE_FLOWER) {
diff --git a/TCPServer.h b/TCPServer.h
index 4d0055d..ccde6d4 100644
--- a/TCPServer.h
+++ b/TCPServer.h
@@ -177,6 +177,8 @@ public:
 
     std::optional<ArucoTag> getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY);
 
+    std::optional<ArucoTag> getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY);
+
     void handleEmergency(int distance, double angle);
 
     /*
diff --git a/utils.cpp b/utils.cpp
index 173fc0f..bd1c25c 100644
--- a/utils.cpp
+++ b/utils.cpp
@@ -92,3 +92,7 @@ std::ostream& operator<<(std::ostream& os, const ArucoTag& tag) {
     os << "ArucoTag{id=" << tag.id() << ", name=" << tag.name() << ", pos=[" << tag.pos()[0] << ", " << tag.pos()[1] << "], rot=[" << tag.rot()[0] << ", " << tag.rot()[1] << ", " << tag.rot()[2] << "]}";
     return os;
 }
+
+double distanceToTag(const ArucoTag& tag) {
+    return std::sqrt(pow(tag.pos()[0], 2) + pow(tag.pos()[1], 2));
+}
\ No newline at end of file
diff --git a/utils.h b/utils.h
index 28ad67e..0563e5a 100644
--- a/utils.h
+++ b/utils.h
@@ -5,6 +5,7 @@
 #include <vector>
 #include <string>
 #include <ostream>
+#include <cmath>
 
 #define PI 3.14159265358979323846
 
@@ -64,3 +65,5 @@ private:
     std::array<float, 3> _rot = {0, 0, 0};
     int nbFind = 0;
 };
+
+double distanceToTag(const ArucoTag& tag);
\ No newline at end of file