go to endpoint

TCPServer.cpp

@@ -205,8 +205,17 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
         file << finishPoint[0] << " " << finishPoint[1];
         file.close();*/
 
-        this->robotPose = {500, 500, -1.57079};
+        // this->robotPose = {500, 500, -1.57079};
+        this->robotPose = {1200, 1700, 1.57079};
+        this->initRobotPose = {1200, 1700, 1.57079};
+        this->endRobotPose = {1200, 1700, 1.57079};
         std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100) + "\n";
+
+        for (int i = 0; i < 3; i++) {
+            this->broadcastMessage(toSend);
+            usleep(200'000);
+        }
+
         this->broadcastMessage(toSend, clientSocket);
     }
     else if (tokens[1] == "strat" && tokens[2] == "start")
@@ -443,6 +452,18 @@ void TCPServer::startGame() {
     // ReSharper disable once CppDFAUnreachableCode
     this->broadcastMessage("strat;arduino;angle;-157");
     this->broadcastMessage("strat;servo_moteur;baisser bras;1");*/
+
+    std::string toSend = "stat;arduino;go;" + std::to_string(this->endRobotPose.pos.x) + "," + std::to_string(this->endRobotPose.pos.y) + "\n";
+    this->broadcastMessage(toSend);
+    isRobotMoving = true;
+    while (this->isRobotMoving) {
+        usleep(500'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+
+    toSend = "start;arduino;angle;" + std::to_string(this->endRobotPose.theta * 100) + "\n";
+    this->broadcastMessage(toSend);
+
     this->broadcastMessage("strat;servo_moteur;clear;1");
 }
 

TCPServer.h

@@ -61,6 +61,9 @@ private:
         float theta;
     } robotPose{};
 
+    Position initRobotPose{};
+    Position endRobotPose{};
+
     std::vector<ArucoTag> arucoTags;
 
     Team team;