From 819ab7459d4bf570e5839b50c77be871968769ee Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Fri, 3 May 2024 18:56:27 +0200
Subject: [PATCH] pince drop

---
 TCPServer.cpp | 36 ++++++++++++++++++++++++++++++------
 TCPServer.h   |  2 ++
 2 files changed, 32 insertions(+), 6 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 01feebd..17af621 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -1268,12 +1268,30 @@ void TCPServer::dropFlowers(const StratPattern sp) {
     this->go(whiteDropPosition);
     usleep(2'000'000);
 
-    for (int i = 0; i < 3; i++) {
+    this->fullyOpenPince(0);
+    this->fullyOpenPince(2);
+    pinceState[0] = NONE;
+    pinceState[2] = NONE;
+
+    usleep(1'000'000);
+
+    this->closePince(0);
+    this->closePince(2);
+
+    usleep(200'000);
+
+    this->fullyOpenPince(1);
+    pinceState[1] = NONE;
+
+    usleep(1'000'000);
+
+    this->closePince(1);
+    /*for (int i = 0; i < 3; i++) {
         this->openPince(i);
         pinceState[i] = NONE;
         this->sendPoint(4);
     }
-    usleep(1'000'000);
+    usleep(1'000'000);*/
 
     this->setSpeed(200);
 
@@ -1293,21 +1311,21 @@ void TCPServer::go3Plants(const StratPattern sp) {
 
     double angle;
     if (sp == TAKE_3_PLANT_TOP_1) {
-        checkpoint = {700, 700};
+        checkpoint = {600, 700};
         plantPosition = {950, 700};
         angle = 0;
     }
     else if (sp == TAKE_3_PLANT_TOP_2) {
-        checkpoint = {700, 700};
+        checkpoint = {600, 700};
         plantPosition = {1100, 700};
         angle = 0;
     }
     else if (sp == TAKE_3_PLANT_BOTTOM_1) {
-        checkpoint = {700, 1300};
+        checkpoint = {600, 1300};
         plantPosition = {950, 1300};
         angle = 0;
     } else if (sp == TAKE_3_PLANT_BOTTOM_2) {
-        checkpoint = {700, 1300};
+        checkpoint = {600, 1300};
         plantPosition = {1100, 1300};
         angle = 0;
     } else {
@@ -1340,6 +1358,8 @@ void TCPServer::go3Plants(const StratPattern sp) {
 
     this->go(checkpoint);
     awaitRobotIdle();
+
+    this->transportBras();
 }
 
 void TCPServer::removePot(StratPattern sp) {
@@ -1497,6 +1517,10 @@ void TCPServer::openPince(int pince) {
     this->broadcastMessage("strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n");
 }
 
+void TCPServer::fullyOpenPince(int pince) {
+    this->broadcastMessage("strat;servo_moteur;ouvrir total pince;" + std::to_string(pince) + "\n");
+}
+
 void TCPServer::middlePince(int pince) {
     this->broadcastMessage("strat;servo_moteur;middle pince;" + std::to_string(pince) + "\n");
 }
diff --git a/TCPServer.h b/TCPServer.h
index 91f94c8..6923cc1 100644
--- a/TCPServer.h
+++ b/TCPServer.h
@@ -266,6 +266,8 @@ public:
 
     void openPince(int pince);
 
+    void fullyOpenPince(int pince);
+
     void middlePince(int pince);
 
     void closePince(int pince);