From 6009a894625975f7d1c93a13cf685bfdf09d79e2 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Fri, 3 May 2024 14:47:14 +0200
Subject: [PATCH] pince

---
 TCPServer.cpp | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index aa08106..9167fe7 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -876,6 +876,11 @@ void TCPServer::goEnd() {
     this->rotate(this->endRobotPose.theta);
     awaitRobotIdle();
 
+    for (int i = 0 ; i < 3; i++) {
+        this->middlePince(i);
+        usleep(50'000);
+    }
+
     this->broadcastMessage("strat;all;end;1");
 }
 
@@ -941,12 +946,19 @@ void TCPServer::findAndGoFlower(const StratPattern sp) {
     }
 
     if (tag.has_value()) {
-        if (pinceState[1] == NONE) {
+        /*if (pinceState[1] == NONE) {
             goToAruco(tag.value(), 1);
         } else if (pinceState[2] == NONE) {
             goToAruco(tag.value(), 2);
         } else if (pinceState[0] == NONE) {
             goToAruco(tag.value(), 0);
+        }*/
+        if (pinceState[0] == NONE) {
+            goToAruco(tag.value(), 0);
+        } else if (pinceState[2] == NONE) {
+            goToAruco(tag.value(), 2);
+        } else if (pinceState[1] == NONE) {
+            goToAruco(tag.value(), 1);
         }
     }
 }