add test

TCPServer.cpp

@@ -188,7 +188,6 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
                 finishPoint[0] = 400;
                 finishPoint[1] = 400;
                 finishPoint[2] = 3.1415;
-
                 break;
             case 6:
                 this->team = YELLOW;
@@ -201,6 +200,13 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
                 break;
 
             default:
+                this->team = TEST;
+                spawnPoint[0] = 1200;
+                spawnPoint[1] = 1800;
+                spawnPoint[2] = 0;
+                finishPoint[0] = 1200;
+                finishPoint[1] = 1800;
+                finishPoint[2] = 0;
                 return;
         }
 
@@ -229,6 +235,9 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
             case YELLOW:
                 std::thread([this]() { this->startGameYellowTeam(); }).detach();
                 break;
+            case TEST:
+                std::thread([this]() { this->startGameTest(); }).detach();
+                break;
         }
     }
     else if (tokens[0] == "aruco" && tokens[2] == "get aruco") {
@@ -363,7 +372,7 @@ void TCPServer::checkIfAllClientsReady()
 
 void TCPServer::startGameBlueTeam() {
     // TODO redo that part because that give point to the other team
-    this->broadcastMessage("strat;arduino;speed;200\n");
+    this->broadcastMessage("strat;arduino;speed;130\n");
     this->broadcastMessage("strat;servo_moteur;check panneau;7\n");
     usleep(100'000);
     std::string toSend;
@@ -377,6 +386,9 @@ void TCPServer::startGameBlueTeam() {
     this->broadcastMessage(toSend);
     awaitRobotIdle();
 
+    this->broadcastMessage("strat;arduino;angle;0");
+    awaitRobotIdle();
+
     this->broadcastMessage("strat;servo_moteur;check panneau;7\n");
     usleep(100'000);
 
@@ -389,6 +401,9 @@ void TCPServer::startGameBlueTeam() {
     this->broadcastMessage(toSend);
     awaitRobotIdle();
 
+    this->broadcastMessage("strat;arduino;angle;0");
+    awaitRobotIdle();
+
     this->broadcastMessage("strat;servo_moteur;check panneau;7\n");
     usleep(100'000);
 
@@ -396,10 +411,12 @@ void TCPServer::startGameBlueTeam() {
     this->broadcastMessage(toSend);
     awaitRobotIdle();
 
+    this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n");
+
     this->broadcastMessage("strat;arduino;angle;104\n");
     awaitRobotIdle();
 
-    this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n");
+    this->broadcastMessage("strat;arduino;speed;200\n");
 
     int timeout = 0;
     ArucoTag tag;
@@ -607,7 +624,7 @@ void TCPServer::startGameBlueTeam() {
     found = false;
     while (!found) {
         for (const auto & arucoTag : this->arucoTags) {
-            if (TCPUtils::endsWith(arucoTag.name(), "flower")) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
                 if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
                     tag = arucoTag;
                     found = true;
@@ -647,7 +664,7 @@ void TCPServer::startGameBlueTeam() {
     found = false;
     while (!found) {
         for (const auto & arucoTag : this->arucoTags) {
-            if (TCPUtils::endsWith(arucoTag.name(), "flower")) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
                 if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
                     tag = arucoTag;
                     found = true;
@@ -687,7 +704,7 @@ void TCPServer::startGameBlueTeam() {
     found = false;
     while (!found) {
         for (const auto & arucoTag : this->arucoTags) {
-            if (TCPUtils::endsWith(arucoTag.name(), "flower")) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
                 if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
                     tag = arucoTag;
                     found = true;
@@ -793,7 +810,7 @@ void TCPServer::startGameBlueTeam() {
     found = false;
     while (!found) {
         for (const auto & arucoTag : this->arucoTags) {
-            if (TCPUtils::endsWith(arucoTag.name(), "flower")) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
                 if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
                     tag = arucoTag;
                     found = true;
@@ -833,7 +850,7 @@ void TCPServer::startGameBlueTeam() {
     found = false;
     while (!found) {
         for (const auto & arucoTag : this->arucoTags) {
-            if (TCPUtils::endsWith(arucoTag.name(), "flower")) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
                 if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
                     tag = arucoTag;
                     found = true;
@@ -873,7 +890,7 @@ void TCPServer::startGameBlueTeam() {
     found = false;
     while (!found) {
         for (const auto & arucoTag : this->arucoTags) {
-            if (TCPUtils::endsWith(arucoTag.name(), "flower")) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
                 if (arucoTag.pos().first[0] < 800 && arucoTag.pos().first[0] > 300 && arucoTag.pos().first[1] < 300 && arucoTag.pos().first[1] > -300) {
                     tag = arucoTag;
                     found = true;
@@ -995,6 +1012,221 @@ void TCPServer::startGameYellowTeam() {
     this->broadcastMessage("strat;servo_moteur;clear;1");
 }
 
+void TCPServer::startGameTest() {
+    this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
+    this->broadcastMessage("strat;servo_moteur;fermer pince;1\n");
+    this->broadcastMessage("strat;servo_moteur;fermer pince;2\n");
+    this->broadcastMessage("strat;servo_moteur;ouvrir pince;0\n");
+    // TODO set to 200 when the robot is ready
+    this->broadcastMessage("strat;arduino;speed;200\n");
+
+    this->broadcastMessage("strat;aruco;get aruco;1\n");
+
+    int timeout = 0;
+    ArucoTag tag;
+    bool found = false;
+    while (!found) {
+        for (const auto & arucoTag : this->arucoTags) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
+                tag = arucoTag;
+                found = true;
+                break;
+            }
+        }
+
+        if (!found) {
+            this->broadcastMessage("start;aruco;get aruco;1");
+            usleep(500'000);
+            timeout++;
+            if (timeout > 10) {
+                return;
+            }
+        }
+    }
+
+    // goToAruco(tag, 1);
+
+    if (pinceState[1] == NONE) {
+        goToAruco(tag, 1);
+    } else if (pinceState[2] == NONE) {
+        goToAruco(tag, 2);
+    } else if (pinceState[0] == NONE) {
+        goToAruco(tag, 0);
+    } else {
+        return;
+    }
+
+    // pi/4
+    this->broadcastMessage("strat;arduino;angle;314\n");
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+
+    // ReSharper disable once CppDFAUnreachableCode
+
+    this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
+
+    usleep(2'000'000);
+    arucoTags.clear();
+    this->broadcastMessage("strat;aruco;get aruco;1\n");
+
+    found = false;
+    timeout = 0;
+    while (!found) {
+        for (const auto & arucoTag : this->arucoTags) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
+                tag = arucoTag;
+                found = true;
+                break;
+            }
+        }
+
+        if (!found) {
+            this->broadcastMessage("start;aruco;get aruco;1");
+            usleep(500'000);
+            timeout++;
+            if (timeout > 10) {
+                return;
+            }
+        }
+    }
+
+    if (pinceState[1] == NONE) {
+        goToAruco(tag, 1);
+    } else if (pinceState[2] == NONE) {
+        goToAruco(tag, 2);
+    } else if (pinceState[0] == NONE) {
+        goToAruco(tag, 0);
+    } else {
+        return;
+    }
+
+    this->broadcastMessage("strat;arduino;angle;157\n");
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+
+    // ReSharper disable once CppDFAUnreachableCode
+
+    this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
+
+    usleep(2'000'000);
+    arucoTags.clear();
+    this->broadcastMessage("strat;aruco;get aruco;1\n");
+
+    found = false;
+    timeout = 0;
+    while (!found) {
+        for (const auto & arucoTag : this->arucoTags) {
+            if (TCPUtils::endWith(arucoTag.name(), "flower")) {
+                tag = arucoTag;
+                found = true;
+                break;
+            }
+        }
+
+        if (!found) {
+            this->broadcastMessage("start;aruco;get aruco;1");
+            usleep(500'000);
+            timeout++;
+            if (timeout > 10) {
+                return;
+            }
+        }
+    }
+
+    if (pinceState[1] == NONE) {
+        goToAruco(tag, 1);
+    } else if (pinceState[2] == NONE) {
+        goToAruco(tag, 2);
+    } else if (pinceState[0] == NONE) {
+        goToAruco(tag, 0);
+    } else {
+        return;
+    }
+
+    // go to jardinière
+
+    this->broadcastMessage("strat;servo_moteur;lever bras;1\n");
+
+    std::string toSend = "strat;arduino;go;762,300\n";
+    this->broadcastMessage(toSend);
+    usleep(200'000);
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+
+    this->broadcastMessage("strat;arduino;angle;157\n");
+    usleep(200'000);
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+
+    this->broadcastMessage("strat;arduino;speed;150\n");
+    this->broadcastMessage("strat;arduino;go;762,0\n");
+    usleep(1'000'000);
+
+    this->broadcastMessage("strat;servo_moteur;ouvrir pince;0\n");
+    pinceState[0] = NONE;
+    this->broadcastMessage("strat;servo_moteur;ouvrir pince;2\n");
+    pinceState[2] = NONE;
+    usleep(200'000);
+
+    this->broadcastMessage("strat;servo_moteur;fermer pince;0\n");
+    this->broadcastMessage("strat;servo_moteur;fermer pince;2\n");
+    this->broadcastMessage("strat;servo_moteur;ouvrir pince;1\n");
+    pinceState[1] = NONE;
+    usleep(200'000);
+
+    this->broadcastMessage("strat;arduino;speed;200\n");
+
+    toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->endRobotPose.pos.y)) + "\n";
+    this->broadcastMessage(toSend);
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+
+    toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>(this->endRobotPose.theta * 100)) + "\n";
+    this->broadcastMessage(toSend);
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'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;baisser bras;1");
+
+    toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->endRobotPose.pos.y)) + "\n";
+    this->broadcastMessage(toSend);
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+    toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>(this->endRobotPose.theta * 100));
+    this->broadcastMessage(toSend);
+    isRobotIdle = 0;
+    while (this->isRobotIdle < 3) {
+        usleep(200'000);
+        this->broadcastMessage("strat;arduino;get state;1\n");
+    }
+
+    this->broadcastMessage("strat;servo_moteur;clear;1");
+}
+
 
 void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
     double robotPosX = this->robotPose.pos.x;

TCPServer.h

@@ -21,7 +21,8 @@ struct ClientTCP
 
 enum Team {
     YELLOW,
-    BLUE
+    BLUE,
+    TEST
 };
 
 class TCPServer; // Forward declaration
@@ -99,6 +100,8 @@ public:
 
     void startGameYellowTeam();
 
+    void startGameTest();
+
     void goToAruco(const ArucoTag &arucoTag, int pince);
 
     void askArduinoPos();