criando_classe_goleiro

Armorial-PSEL.pro

@@ -44,6 +44,7 @@ SOURCES += \
         main.cpp \
         src/entities/actuator/actuator.cpp \
         src/entities/coach/coach.cpp \
+        src/entities/player/gk_control.cpp \
         src/entities/player/player.cpp \
         src/entities/vision/vision.cpp \
         src/entities/worldmap/worldmap.cpp \
@@ -67,6 +68,7 @@ HEADERS += \
     include/proto/ssl_vision_wrapper.pb.h \
     src/entities/actuator/actuator.h \
     src/entities/coach/coach.h \
+    src/entities/player/gk_control.h \
     src/entities/player/player.h \
     src/entities/vision/vision.h \
     src/entities/worldmap/worldmap.h \

main.cpp

@@ -40,7 +40,7 @@ int main(int argc, char *argv[])
 
     // Create WorldMap instance and connect it with the Vision module to receive detection packets
     // from ball and field.
-    WorldMap *worldMap = new WorldMap(false);
+    WorldMap *worldMap = new WorldMap(true);
     QObject::connect(vision, &Vision::sendFieldDetection, worldMap, &WorldMap::updateFieldDetection);
     QObject::connect(vision, &Vision::sendBallDetection, worldMap, &WorldMap::updateBallDetection);
 

src/entities/coach/coach.cpp

@@ -24,10 +24,11 @@
 #define BALL_RADIUS 0.0215
 #define BALL_DIAMETER (2.0F * BALL_RADIUS)
 #define ROBOT_RADIUS 0.09
-#define BALL_SPEED 3.0;
+#define BALL_SPEED 3.0
+#define SEMICIRCLE_RADIUS 1.5
 
 Coach::Coach(const QMap<bool, QList<Player*>>& players, WorldMap* worldMap)
-    : _players(players), _worldMap(worldMap), contador(0)
+    : _players(players), _worldMap(worldMap), contador(0), estado(0)
 {
     // Create QTimer and connects to the runCoach() slot
     _actuatorTimer = new QTimer(this);
@@ -37,6 +38,7 @@ Coach::Coach(const QMap<bool, QList<Player*>>& players, WorldMap* worldMap)
     // variavel para armazenar posição e direção da bola
     _lastBallPosition = QVector2D(0,0);
     _ballDirection = QVector2D(0,0);
+    _gk_control = new gk_control(worldMap);
 
 }
 
@@ -69,84 +71,77 @@ void Coach::updateDataBall() {
 
 }
 
-int estado = 0;
-int haskicked = 0;
-int var= 0;
+QVector2D Coach::calculaSemiCircle(const QVector2D& ballPosition, float radius) {
 
-void Coach::runCoach() {
+    //pega o centro do gol
+    QVector2D goalCenter = getWorldMap()->ourGoalCenter();
 
-    if(contador >= 30){
-        updateDataBall();
-        spdlog :: info("testando");
-        contador = 0;
-    }
+    //calcula o angulo do semi circulo
+    float angle = atan2(ballPosition.y() - goalCenter.y(), ballPosition.x() - goalCenter.x());
 
-    QVector2D balloPosition = getWorldMap()->ballPosition();
-    // QVector2D previousBallPosition = ballPosition;
+    // calcula o x e y do target position
+    float x = radius * cos(angle);
+    float y = radius * sin(angle);
 
-    const QVector2D pontoCerto(3.0f, 2.0f);
-    //const QVector2D pontoteste1(2.4f, 1.6f);
+    // retorna target position
+    return QVector2D(goalCenter.x() + x, goalCenter.y() + y);
 
-    // float distP0Ball = getPlayer(BLUE,2).value()->getPosition().distanceToPoint(ballPosition);
-    // float distP1Ball = getPlayer(YELLOW,0).value()->getPosition().distanceToPoint(ballPosition);
-    float distP2Ball = getPlayer(YELLOW,4).value()->getPosition().distanceToPoint(balloPosition);
+}
 
-    getPlayer(YELLOW,0).value()-> goTo(pontoCerto);
-    getPlayer(YELLOW,0).value()-> rotateTo(balloPosition);
-    getPlayer(BLUE,2).value()->rotateTo(pontoCerto);
-    getPlayer(BLUE,2).value()->kick(3.0,false);
+int haskicked = 0;
+int var= 0;
+int gol = 0;
 
+void Coach::runCoach() {
 
-    if (estado == 0){ //estado inicial player0 vai para bola player2 vai para o target
+    // if(contador >= 30){
+    //     updateDataBall();
+    //     //spdlog :: info("testando");
+    //     contador = 0;
+    // }
 
-        getPlayer(BLUE,2).value()->goTo(balloPosition);
-        getPlayer(BLUE,2).value()->dribble(true);
+    QVector2D ballPosition = getWorldMap()->ballPosition();
+    //const QVector2D pontoCerto(3.0f, 2.0f);
+    //float distP2Ball = getPlayer(YELLOW,4).value()->getPosition().distanceToPoint(balloPosition);
 
+    // getPlayer(YELLOW,0).value()-> goTo(pontoCerto);
+    // getPlayer(YELLOW,0).value()-> rotateTo(balloPosition);
+    // getPlayer(BLUE,2).value()->rotateTo(pontoCerto);
+    // getPlayer(BLUE,2).value()->kick(3.0,false);
 
-        if(getPlayer(BLUE,2).value()->getPosition().distanceToPoint(balloPosition) <= (BALL_RADIUS +  ROBOT_DIAMETER)){
+    // if(gol ==0){
 
-            getPlayer(BLUE,2).value()->kick(3.0,false);
+    //     QVector2D targetPosition = calculaSemiCircle(_worldMap->ballPosition(), SEMICIRCLE_RADIUS);  // Replace with your function
+    //     Player *p = getPlayer(YELLOW, 4).value();
+    //     p->goTo(targetPosition);
+    //     p->rotateTo(_worldMap->ballPosition());
 
-            while(var < 10){
-            getPlayer(BLUE,2).value()->kick(3.0,false);
-            var++;
-            }
-            if(var == 10 && haskicked == 0){
-            estado = 1;
-            }
-        }
+    // }
 
+    QMap<quint8, std::optional<Player *>> players;
+    for (quint8 playerId = 0; playerId < 6; playerId++){
+        players.insert(playerId, getPlayer(BLUE, playerId));
     }
 
-    if (estado == 1 && getPlayer(BLUE,2).value()->getPosition().distanceToPoint(balloPosition) >= (ROBOT_DIAMETER+ ROBOT_DIAMETER)){
-
-        //spdlog :: info("testando");
-        if(haskicked == 0){
-            haskicked = 1;
-        if (distP2Ball >= 0.3f && haskicked == 1){
+    for (quint8 playerId : players.keys()){
+        if(players.value(playerId).has_value()){
+            if(playerId == 0){
+                _gk_control -> setPlayer(players.value(playerId).value());
+                if(getWorldMap() -> isBallInsideOurPenaltyArea(ballPosition)){ //se a bola tiver dentro da area passa
+                    _gk_control->pass(QVector2D(-1.5f, -2.0f));
+                    spdlog :: info("testando");
+
+            } else { // se não defende
+                //defend
+                _gk_control -> defend(); //metodo defende
+            }
 
-            estado = 2;
+        } else if (playerId == 1){
+            players.value(playerId).value()->rotateTo(ballPosition);
+        } else{
 
         }
-        }
     }
-
-    if(estado == 2){
-
-
-        //spdlog::info("ballDirection ({}, {})", _ballDirection.x(), _ballDirection.y());
-
-        //float distanceToIntercept = redimensionarDistancia(_worldMap->ballPosition(), _worldMap->ballVelocity(), _worldMap->goalPosition());
-        float distanceToIntercept = 0.8;
-        QVector2D desiredPosition = _worldMap->ballPosition() + _ballDirection * distanceToIntercept;
-
-        //spdlog :: info("testando");
-        Player *p = getPlayer(YELLOW, 4).value();
-        p->goTo(desiredPosition);
-        p->rotateTo(_worldMap->ballPosition());
-    }
-    contador++;
- }
-
-
+  }
+}
 

src/entities/coach/coach.h

@@ -30,6 +30,8 @@
 #include <src/entities/player/player.h>
 #include <src/entities/worldmap/worldmap.h>
 
+#include <src/entities/player/gk_control.h>
+
 #include <spdlog/spdlog.h>
 
 #define COACH_ITERATION_INTERVAL_MS 16
@@ -84,6 +86,9 @@ class Coach : public QObject
     QMap<bool, QList<Player*>> _players;
     WorldMap* _worldMap;
     int contador = 0;
+    int estado = 0;
+    QVector2D calculaSemiCircle(const QVector2D& ballPosition, float radius);
+    gk_control *_gk_control;
 
 
 private slots:

src/entities/player/gk_control.cpp

@@ -0,0 +1,29 @@
+#include "gk_control.h"
+
+gk_control::gk_control(WorldMap *worldMap)
+{
+    _worldMap = worldMap;
+    _player = nullptr;
+}
+
+void gk_control::setPlayer (Player *player)
+{
+    _player = player;
+}
+
+void gk_control::defend()
+{
+    if(_player != nullptr){
+        _player->goTo(_worldMap->ourGoalCenter());
+        _player->rotateTo(_worldMap->ballPosition());
+    }
+}
+
+void gk_control::pass(QVector2D target)
+{
+  //faz o passe para um player especifico
+    if(_player != nullptr){
+       _player ->goTo(target);//
+    }
+}
+

src/entities/player/gk_control.h

@@ -0,0 +1,25 @@
+#ifndef GK_CONTROL_H
+#define GK_CONTROL_H
+
+#include <src/entities/player/player.h>
+#include <src/entities/worldmap/worldmap.h>
+
+class gk_control
+{
+public:
+
+    gk_control(WorldMap *worldMap);
+
+    void setPlayer(Player *player);
+    void defend();
+    void pass(QVector2D target);
+
+    QVector2D getGoalCircleCenter();
+
+private:
+    Player *_player;
+    WorldMap *_worldMap;
+
+};
+
+#endif // GK_CONTROL_H

src/entities/player/player.h

@@ -93,6 +93,7 @@ class Player : public QObject
 protected:
     // Mark Coach as a friend class so it can call this methods from Player
     friend class Coach;
+    friend class gk_control;;
 
     /*!
      * \brief Make this Player go to a given target position.

src/entities/worldmap/worldmap.cpp

@@ -64,11 +64,11 @@ float WorldMap::goalWidth() const {
 }
 
 float WorldMap::penaltyDepth() const {
-    return 0.15f;
+    return 2.0f;
 }
 
 float WorldMap::penaltyWidth() const {
-    return 0.7f;
+    return 1.0f;
 }
 
 float WorldMap::penaltyMarkDistanceFromGoal() const {
@@ -134,6 +134,56 @@ QVector2D WorldMap::ballPosition() const {
     return QVector2D(_ball.x() / 1000.0f, _ball.y() / 1000.0f);
 }
 
+bool WorldMap::isBallInsideOurPenaltyArea(QVector2D position) const{
+
+    if(!playingLeftSide()){
+        QVector2D topLeft(ourGoalCenter().x() - penaltyWidth(), penaltyDepth() / 2);
+        QVector2D bottomRight(ourGoalCenter().x(), -(penaltyDepth() / 2));
+
+        if(position.x() >= topLeft.x() && position.x() <= bottomRight.x()){
+            if(position.y() <= topLeft.y() && position.y() >= bottomRight.y()){
+                return true;
+            }
+        }
+        return false;
+    } else{
+        QVector2D topLeft(ourGoalCenter().x(), penaltyDepth() / 2);
+        QVector2D bottomRight(ourGoalCenter().x() + penaltyWidth(), -(penaltyDepth() / 2));
+
+        if(position.x() >= topLeft.x() && position.x()<= bottomRight.x()){
+            if(position.y() <= topLeft.y() && position.y() >= bottomRight.y()){
+                return true;
+            }
+        }
+        return false;
+    }
+}
+
+bool WorldMap :: isBallInsideTheirPenaltyArea(QVector2D position) const{
+
+    if(playingLeftSide()){
+        QVector2D topLeft(theirGoalCenter().x() - penaltyWidth(), penaltyDepth() / 2);
+        QVector2D bottomRight(theirGoalCenter().x(), -(penaltyDepth() / 2));
+
+        if(position.x() >= topLeft.x() && position.x() <= bottomRight.x()){
+            if(position.y() <= topLeft.y() && position.y() >= bottomRight.y()){
+                return true;
+            }
+        }
+        return false;
+    } else{
+        QVector2D topLeft(theirGoalCenter().x(), penaltyDepth() / 2);
+        QVector2D bottomRight(theirGoalCenter().x() + penaltyWidth(), -(penaltyDepth() / 2));
+
+        if(position.x() >= topLeft.x() && position.x()<= bottomRight.x()){
+            if(position.y() <= topLeft.y() && position.y() >= bottomRight.y()){
+                return true;
+            }
+        }
+        return false;
+    }
+}
+
 void WorldMap::updateBallDetection(const SSL_DetectionBall &ball)
 {
     _ball = ball;

src/entities/worldmap/worldmap.h

@@ -154,6 +154,10 @@ class WorldMap : public QObject
      */
     QVector2D ballPosition() const;
 
+    bool isBallInsideOurPenaltyArea(QVector2D position) const;
+
+    bool isBallInsideTheirPenaltyArea(QVector2D position) const;
+
 private:
     // Internal variables
     SSL_GeometryData _field;