Merge remote-tracking branch 'origin/master'

TCPServer.h

@@ -6,27 +6,62 @@
 #include <unistd.h>
 #include <cstring>
 #include <thread>
+#include <utility>
 #include <vector>
 #include <algorithm>
 #include <atomic>
-#include <map>
-#include <cmath>
 #include <fstream>
 #include <optional>
 
 #include "utils.h"
 
+#define MAX_SPEED 200
+#define MIN_SPEED 150
+
 struct ClientTCP
 {
     std::string name;
     int socket = -1;
     bool isReady = false;
+
+    ClientTCP() = default;
+
+    explicit ClientTCP(std::string name, int socket = -1) : name(std::move(name)), socket(socket) {}
 };
 
 enum Team {
-    YELLOW,
     BLUE,
-    TEST
+    YELLOW,
+    TEST,
+};
+
+enum StratPattern {
+    TURN_SOLAR_PANNEL_1,
+    TURN_SOLAR_PANNEL_2,
+    TURN_SOLAR_PANNEL_3,
+    TAKE_FLOWER_BOTTOM,
+    TAKE_FLOWER_TOP,
+    DROP_PURPLE_FLOWER,
+    DROP_WHITE_FLOWER_J1,
+    DROP_WHITE_FLOWER_J2,
+    GO_END,
+    GET_LIDAR_POS,
+    CHECKPOINT_MIDDLE,
+    CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER,
+    TAKE_3_PLANT_BOTTOM_1,
+    TAKE_3_PLANT_BOTTOM_2,
+    TAKE_3_PLANT_TOP_1,
+    TAKE_3_PLANT_TOP_2,
+    DROP_FLOWER_J1,
+    DROP_FLOWER_J2,
+    REMOVE_POT_J2,
+    DROP_FLOWER_BASE_1,
+    DROP_FLOWER_BASE_2,
+    SLEEP_1S,
+    SLEEP_5S,
+    SLEEP_10S,
+    ROTATE_0,
+    ROTATE_270,
 };
 
 class TCPServer; // Forward declaration
@@ -55,9 +90,13 @@ private:
     std::atomic<bool> _shouldStop = false; // Flag to indicate if the server should stop
     std::vector<ClientTCP> clients; // Store connected clients
 
-    PinceState pinceState[3] = {NONE, NONE, NONE};
+    std::array<PinceState, 3> pinceState = {NONE, NONE, NONE};
     int isRobotIdle = 0;
 
+    bool gameStarted = false;
+
+    std::chrono::time_point<std::chrono::system_clock> gameStart;
+
     int speed = 0;
 
     struct Position {
@@ -66,15 +105,80 @@ private:
             float y;
         } pos;
         float theta;
-    } robotPose{};
+    };
 
+    Position robotPose{};
     Position initRobotPose{};
     Position endRobotPose{};
+    Position lidarCalculatePos{};
 
     std::vector<ArucoTag> arucoTags;
 
     Team team;
 
+    std::vector<StratPattern> stratPatterns = {
+        TURN_SOLAR_PANNEL_1,
+        TURN_SOLAR_PANNEL_2,
+        TURN_SOLAR_PANNEL_3,
+        CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER,
+
+        /*TAKE_FLOWER_BOTTOM,
+        TAKE_FLOWER_BOTTOM,
+        TAKE_FLOWER_BOTTOM,*/
+
+        /*TAKE_3_PLANT_BOTTOM_1,
+        DROP_FLOWER_BASE_2,*/
+
+        TAKE_3_PLANT_BOTTOM_1,
+        GET_LIDAR_POS,
+        REMOVE_POT_J2,
+        DROP_FLOWER_J2,
+        ROTATE_270,
+
+        /*DROP_PURPLE_FLOWER,
+        DROP_WHITE_FLOWER_J1,*/
+        // GET_LIDAR_POS,
+        /*TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,*/
+
+        TAKE_3_PLANT_TOP_1,
+        GET_LIDAR_POS,
+        DROP_FLOWER_J1,
+
+        /*DROP_PURPLE_FLOWER,
+        DROP_WHITE_FLOWER_J2,*/
+        // GET_LIDAR_POS,
+        /*TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,*/
+        //TAKE_3_PLANT_TOP,
+        /*DROP_WHITE_FLOWER_J2,
+        DROP_PURPLE_FLOWER,*/
+
+        TAKE_3_PLANT_TOP_2,
+        DROP_FLOWER_BASE_1,
+
+        GO_END
+    };
+
+    // This is the index of the current pattern
+    int whereAmI = 0;
+
+    bool stopEmergency = false;
+    bool handleEmergencyFlag = false;
+
+    bool awaitForLidar = false;
+
+    std::thread gameThread;
+
+    int lidarSocket = -1;
+    int arduinoSocket = -1;
+
+    int lidarGetPosTimeout = 0;
+
+    std::string lastArduinoCommand{};
+
 public:
     explicit TCPServer(int port);
 
@@ -102,9 +206,7 @@ public:
 
     void checkIfAllClientsReady();
 
-    void startGameBlueTeam();
-
-    void startGameYellowTeam();
+    void startGame();
 
     void startGameTest();
 
@@ -114,25 +216,109 @@ public:
 
     [[nodiscard]] bool shouldStop() const;
 
-    void awaitRobotIdle();
+    int awaitRobotIdle();
 
-    void handleArucoTag(ArucoTag &tag);
+    void handleArucoTag(const ArucoTag &tag);
 
     std::optional<ArucoTag> getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY);
 
+    std::optional<ArucoTag> getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY);
+
+    std::vector<PinceState> getNotFallenFlowers() const;
+
+    void handleEmergency(int distance, double angle);
+
+    /*
+     * Start Strategy function
+    */
+    void goAndTurnSolarPanel(StratPattern sp);
+
+    void findAndGoFlower(StratPattern sp);
+
+    void goEnd();
+
+    void dropPurpleFlowers();
+
+    void dropWhiteFlowers(StratPattern sp);
+
+    void getLidarPos();
+
+    void checkpoint(StratPattern sp);
+
+    void dropJardiniereFlowers(StratPattern sp);
+
+    void dropBaseFlowers(StratPattern sp);
+
+    void go3Plants(StratPattern sp);
+
+    void removePot(StratPattern sp);
+    /*
+     *  End Strategy function
+     */
+
     void startTestAruco(int pince);
 
     // Call to broadcast
     void setSpeed(int speed);
 
+    void setMaxSpeed();
+
+    void setMinSpeed();
+
     template<class X, class Y>
     void go(X x, Y y);
 
+    template<class X>
+    void go(std::array<X, 2> data);
+
     template<class X>
     void rotate(X angle);
 
     template<class X, class Y>
     void transit(X x,Y y, int endSpeed);
 
+    template<class X>
+    void transit(std::array<X, 2> data, int endSpeed);
+
+    template<class X, class Y, class Z>
+    void setPosition(X x, Y y, Z theta, int clientSocket = -1);
+
+    template<class X>
+    void setPosition(std::array<X, 3> data, int clientSocket = -1);
+
+    void setPosition(Position pos, int clientSocket = -1);
+
+    template<class X, class Y, class Z>
+    void setPosition(X x, Y y, Z theta, const std::string &toSend);
+
+    template<class X>
+    void setPosition(std::array<X, 3> data, const std::string &toSend);
+
+    void setPosition(Position pos, const std::string &toSend);
+
+    void openPince(int pince);
+
+    void fullyOpenPince(int pince);
+
+    void middlePince(int pince);
+
+    void closePince(int pince);
+
+    void baisserBras();
+
+    void leverBras();
+
+    void transportBras();
+
+    void checkPanneau(int servo_moteur);
+
+    void uncheckPanneau(int servo_moteur);
+
+    void askLidarPosition();
+
+    void sendPoint(int point);
+
+    void setTeam(Team team);
+
     ~TCPServer();
 };

main.cpp

@@ -5,14 +5,12 @@
 std::atomic<bool> shouldStop = false;
 
 void signalHandler( int signum ) {
-
     shouldStop = true;
-
-    exit(signum);
 }
 
 int main(int argc, char* argv[]) {
     signal(SIGINT, signalHandler);
+    signal(SIGTERM, signalHandler);
 
     CLParser clParser(argc, argv);
 

utils.cpp

@@ -88,18 +88,11 @@ int ArucoTag::getNbFind() const {
     return nbFind;
 }
 
-FlowerAruco::FlowerAruco() : tag(nullptr), _realPos({0, 0}) {
-
+std::ostream& operator<<(std::ostream& os, const ArucoTag& tag) {
+    os << "ArucoTag{id=" << tag.id() << ", name=" << tag.name() << ", pos=[" << tag.pos()[0] << ", " << tag.pos()[1] << "], rot=[" << tag.rot()[0] << ", " << tag.rot()[1] << ", " << tag.rot()[2] << "]}";
+    return os;
 }
 
-FlowerAruco::FlowerAruco(ArucoTag *tag) : tag(tag), _realPos({0, 0}) {
-
-}
-
-ArucoTag *FlowerAruco::getTag() const {
-    return tag;
-}
-
-std::array<float, 2> FlowerAruco::getPos() const {
-    return _realPos;
-}
+double distanceToTag(const ArucoTag& tag) {
+    return std::sqrt(pow(tag.pos()[0], 2) + pow(tag.pos()[1], 2));
+}

utils.h

@@ -4,12 +4,15 @@
 #include <utility>
 #include <vector>
 #include <string>
+#include <ostream>
+#include <cmath>
 
 #define PI 3.14159265358979323846
 
 enum PinceState {
     WHITE_FLOWER,
     PURPLE_FLOWER,
+    FLOWER,
     NONE
 };
 
@@ -26,10 +29,12 @@ namespace TCPUtils {
 class ArucoTag {
 
 public:
-    ArucoTag(int id, std::string  name, std::array<float, 2> pos, std::array<float, 3> rot);
+    ArucoTag(int id, std::string name, std::array<float, 2> pos, std::array<float, 3> rot);
 
     ArucoTag() = default;
 
+    ArucoTag(const ArucoTag& other) = default;
+
     [[nodiscard]] int id() const;
 
     [[nodiscard]] std::string name() const;
@@ -52,27 +57,14 @@ public:
 
     [[nodiscard]] int getNbFind() const;
 
+    friend std::ostream& operator<<(std::ostream& os, const ArucoTag& tag);
+
 private:
     int _id = -1;
     std::string _name;
-    std::array<float, 2> _pos;
-    std::array<float, 3> _rot;
-    int nbFind = 0;
+    std::array<float, 2> _pos = {0, 0};
+    std::array<float, 3> _rot = {0, 0, 0};
+    int nbFind = 1;
 };
 
-class FlowerAruco {
-
-public:
-    FlowerAruco();
-
-    explicit FlowerAruco(ArucoTag* tag);
-
-    [[nodiscard]] ArucoTag* getTag() const;
-
-    [[nodiscard]] std::array<float, 2> getPos() const;
-
-private:
-    ArucoTag* tag;
-
-    std::array<float, 2> _realPos;
-};
+double distanceToTag(const ArucoTag& tag);