From 87f5c6bae568948137de4e339e8f1cb257b6bb95 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Sat, 4 May 2024 10:41:31 +0200
Subject: [PATCH] lidar

---
 TCPServer.cpp | 22 +++++++++++++---------
 TCPServer.h   |  6 ++++--
 2 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 157a07f..4eab726 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -227,6 +227,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
             file << finishPoint[0] << " " << finishPoint[1];
             file.close();
 
+            this->sendToClient("strat;lidar;set team;" + std::to_string(this->team) + "\n", lidarSocket);
+            this->sendToClient("strat;lidar;set beacon;1", lidarSocket);
+
             this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]};
             this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]};
             this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]};
@@ -768,6 +771,7 @@ void TCPServer::awaitRobotIdle() {
             break;
         }
     }
+    usleep(50'000);
 }
 
 void TCPServer::handleArucoTag(const ArucoTag &tag) {
@@ -1146,8 +1150,7 @@ void TCPServer::dropWhiteFlowers(const StratPattern sp) {
 }
 
 void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
-    int previousSpeed = this->speed;
-    this->setMaxSpeed();
+    this->setSpeed(170);
     if (team == BLUE) {
         switch (sp) {
             case TURN_SOLAR_PANNEL_1:
@@ -1227,8 +1230,6 @@ void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
     }
 
     this->sendPoint(5);
-
-    this->setSpeed(previousSpeed);
 }
 
 void TCPServer::dropJardiniereFlowers(const StratPattern sp) {
@@ -1420,10 +1421,12 @@ void TCPServer::go3Plants(const StratPattern sp) {
         return;
     }
 
-    this->go(plantPosition[0]-400, plantPosition[1]);
+    this->setMaxSpeed();
+
+    this->transit(plantPosition[0]-400, plantPosition[1], 150);
     awaitRobotIdle();
 
-    this->setSpeed(170);
+    this->setSpeed(150);
     this->rotate(angle);
     awaitRobotIdle();
 
@@ -1434,8 +1437,9 @@ void TCPServer::go3Plants(const StratPattern sp) {
     }
     usleep(200'000);
 
-    this->go(plantPosition[0], this->robotPose.pos.y);
+    this->transit(plantPosition[0], this->robotPose.pos.y, 130);
     awaitRobotIdle();
+
     usleep(500'000);
 
     for (int i = 0; i < 3; i++) {
@@ -1444,7 +1448,7 @@ void TCPServer::go3Plants(const StratPattern sp) {
     }
     usleep(500'000);
 
-    this->setSpeed(170);
+    this->setSpeed(150);
     this->rotate(angle);
     awaitRobotIdle();
 
@@ -1470,7 +1474,7 @@ void TCPServer::go3Plants(const StratPattern sp) {
 }
 
 void TCPServer::removePot(StratPattern sp) {
-    this->setSpeed(200);
+    this->setMaxSpeed();
     if (team == BLUE) {
         if (sp == REMOVE_POT_J2) {
             this->go(230, 1000);
diff --git a/TCPServer.h b/TCPServer.h
index be3e301..576cd42 100644
--- a/TCPServer.h
+++ b/TCPServer.h
@@ -17,7 +17,7 @@
 
 #include "utils.h"
 
-#define MAX_SPEED 200
+#define MAX_SPEED 170
 #define MIN_SPEED 150
 
 struct ClientTCP
@@ -32,8 +32,8 @@ struct ClientTCP
 };
 
 enum Team {
-    YELLOW,
     BLUE,
+    YELLOW,
     TEST
 };
 
@@ -118,6 +118,7 @@ private:
         TURN_SOLAR_PANNEL_2,
         TURN_SOLAR_PANNEL_3,
         CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER,
+        GET_LIDAR_POS,
 
         /*TAKE_FLOWER_BOTTOM,
         TAKE_FLOWER_BOTTOM,
@@ -128,6 +129,7 @@ private:
         TAKE_3_PLANT_BOTTOM_2,
         REMOVE_POT_J2,
         DROP_FLOWER_J2,
+        GET_LIDAR_POS,
 
         /*DROP_PURPLE_FLOWER,
         DROP_WHITE_FLOWER_J1,*/