From 9225dec74aef5f7dcf39868c2aee56a0644fba1e Mon Sep 17 00:00:00 2001 From: Gabriel Clemente Date: Tue, 16 Jan 2024 17:08:40 -0300 Subject: [PATCH] criando_classe_goleiro --- Armorial-PSEL.pro | 2 + main.cpp | 2 +- src/entities/coach/coach.cpp | 119 ++++++++++++++--------------- src/entities/coach/coach.h | 5 ++ src/entities/player/gk_control.cpp | 29 +++++++ src/entities/player/gk_control.h | 25 ++++++ src/entities/player/player.h | 1 + src/entities/worldmap/worldmap.cpp | 54 ++++++++++++- src/entities/worldmap/worldmap.h | 4 + 9 files changed, 176 insertions(+), 65 deletions(-) create mode 100644 src/entities/player/gk_control.cpp create mode 100644 src/entities/player/gk_control.h diff --git a/Armorial-PSEL.pro b/Armorial-PSEL.pro index 4a0e853..e78fe3e 100644 --- a/Armorial-PSEL.pro +++ b/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 \ diff --git a/main.cpp b/main.cpp index 737c7d7..7291338 100644 --- a/main.cpp +++ b/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); diff --git a/src/entities/coach/coach.cpp b/src/entities/coach/coach.cpp index ba3c947..b4be9f7 100644 --- a/src/entities/coach/coach.cpp +++ b/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>& 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>& 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> 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++; - } - - + } +} diff --git a/src/entities/coach/coach.h b/src/entities/coach/coach.h index 51f0395..9d71222 100644 --- a/src/entities/coach/coach.h +++ b/src/entities/coach/coach.h @@ -30,6 +30,8 @@ #include #include +#include + #include #define COACH_ITERATION_INTERVAL_MS 16 @@ -84,6 +86,9 @@ class Coach : public QObject QMap> _players; WorldMap* _worldMap; int contador = 0; + int estado = 0; + QVector2D calculaSemiCircle(const QVector2D& ballPosition, float radius); + gk_control *_gk_control; private slots: diff --git a/src/entities/player/gk_control.cpp b/src/entities/player/gk_control.cpp new file mode 100644 index 0000000..a3e986a --- /dev/null +++ b/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);// + } +} + diff --git a/src/entities/player/gk_control.h b/src/entities/player/gk_control.h new file mode 100644 index 0000000..0ec0be4 --- /dev/null +++ b/src/entities/player/gk_control.h @@ -0,0 +1,25 @@ +#ifndef GK_CONTROL_H +#define GK_CONTROL_H + +#include +#include + +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 diff --git a/src/entities/player/player.h b/src/entities/player/player.h index 4c2df02..e1239b5 100644 --- a/src/entities/player/player.h +++ b/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. diff --git a/src/entities/worldmap/worldmap.cpp b/src/entities/worldmap/worldmap.cpp index c00aa74..5a8bf27 100644 --- a/src/entities/worldmap/worldmap.cpp +++ b/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; diff --git a/src/entities/worldmap/worldmap.h b/src/entities/worldmap/worldmap.h index 1e362ce..750ee4e 100644 --- a/src/entities/worldmap/worldmap.h +++ b/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;