diff --git a/Armorial-PSEL.pro b/Armorial-PSEL.pro index e78fe3e..f086282 100644 --- a/Armorial-PSEL.pro +++ b/Armorial-PSEL.pro @@ -44,6 +44,8 @@ SOURCES += \ main.cpp \ src/entities/actuator/actuator.cpp \ src/entities/coach/coach.cpp \ + src/entities/player/ak_control.cpp \ + src/entities/player/dk_control.cpp \ src/entities/player/gk_control.cpp \ src/entities/player/player.cpp \ src/entities/vision/vision.cpp \ @@ -68,6 +70,8 @@ HEADERS += \ include/proto/ssl_vision_wrapper.pb.h \ src/entities/actuator/actuator.h \ src/entities/coach/coach.h \ + src/entities/player/ak_control.h \ + src/entities/player/dk_control.h \ src/entities/player/gk_control.h \ src/entities/player/player.h \ src/entities/vision/vision.h \ diff --git a/src/entities/coach/coach.cpp b/src/entities/coach/coach.cpp index 2116631..8703452 100644 --- a/src/entities/coach/coach.cpp +++ b/src/entities/coach/coach.cpp @@ -39,6 +39,8 @@ Coach::Coach(const QMap>& players, WorldMap* worldMap) _lastBallPosition = QVector2D(0,0); _ballDirection = QVector2D(0,0); _gk_control = new gk_control(worldMap); + _dk_control = new dk_control(worldMap); + _ak_control = new ak_control(worldMap); } @@ -71,27 +73,34 @@ void Coach::updateDataBall() { } -QVector2D Coach::calculaSemiCircle(const QVector2D& ballPosition, float radius) { - - //pega o centro do gol - QVector2D goalCenter = getWorldMap()->ourGoalCenter(); - - //calcula o angulo do semi circulo - float angle = atan2(ballPosition.y() - goalCenter.y(), ballPosition.x() - goalCenter.x()); +float distanceToSegment(const QVector2D &point, const QVector2D &segmentStart, const QVector2D &segmentEnd) { + QVector2D segment = segmentEnd - segmentStart; + QVector2D pointToStart = point - segmentStart; + float t = QVector2D::dotProduct(pointToStart, segment) / segment.lengthSquared(); + + if (t < 0.0f) { + return pointToStart.length(); // ponto mais próximo é o início do segmento + } else if (t > 1.0f) { + return (point - segmentEnd).length(); // ponto mais próximo é o final do segmento + } else { + QVector2D closestPoint = segmentStart + t * segment; + return (point - closestPoint).length(); // ponto mais próximo está no meio do segmento + } +} - // calcula o x e y do target position - float x = radius * cos(angle); - float y = radius * sin(angle); +QList Coach::getYellowPositions() { + QList yellowPositions; + for (int i = 0; i < 6; i++) { + std::optional yellowRobotOpt = getPlayer(YELLOW, i); - // retorna target position - return QVector2D(goalCenter.x() + x, goalCenter.y() + y); + if (yellowRobotOpt.has_value()) { + yellowPositions.append(yellowRobotOpt.value()->getPosition()); + } + } + return yellowPositions; } -int haskicked = 0; -int var= 0; -int gol = 0; - void Coach::runCoach() { // if(contador >= 30){ @@ -108,6 +117,10 @@ void Coach::runCoach() { // getPlayer(YELLOW,0).value()-> rotateTo(balloPosition); // getPlayer(BLUE,2).value()->rotateTo(pontoCerto); // getPlayer(BLUE,2).value()->kick(3.0,false); + //player 5 goleiro + //player 0 defensor + //player 4 defensor + //player 3,1,2 atacante QMap> players; for (quint8 playerId = 0; playerId < 6; playerId++){ @@ -125,17 +138,59 @@ void Coach::runCoach() { _gk_control->pass(getPlayer(BLUE, 0).value()); //spdlog :: info("testando"); - } else { + } + else { //defend _gk_control -> defend(); //metodo defende + } + + } + + if (playerId == 0){ //defensor blue 0 + _dk_control -> setDPlayer(players.value(playerId).value()); + players.value(playerId).value()->dribble(true); + + if(ballPosition.x() <= 0.0f && ballPosition.y()>= 0.0f){ + _dk_control->intercept0(getPlayer(BLUE, 1).value()); + } + if(ballPosition.x()>= 0.0f){ + _dk_control->defensePosition0(); + } + + } + if(playerId == 4){ + _dk_control -> setDPlayer(players.value(playerId).value()); + players.value(playerId).value()->dribble(true); + + if(ballPosition.x() <= 0.0f && ballPosition.y() <= 0.0f){ + _dk_control->intercept4(getPlayer(BLUE, 2).value()); + } + if(ballPosition.x()>= 0.0f){ + _dk_control->defensePosition4(); + } + + + + } + if(playerId == 1){ + _ak_control -> setAPlayer(players.value(playerId).value()); + players.value(playerId).value()->dribble(true); + + if(ballPosition.x()<= 0.0f){ + _ak_control->atackposition1(); + } + } - } else if (playerId == 0){ - players.value(playerId).value()->rotateTo(ballPosition); - } else{ + + + + + } + + } - } } diff --git a/src/entities/coach/coach.h b/src/entities/coach/coach.h index 9d71222..b6e4a05 100644 --- a/src/entities/coach/coach.h +++ b/src/entities/coach/coach.h @@ -31,6 +31,8 @@ #include #include +#include +#include #include @@ -87,8 +89,9 @@ class Coach : public QObject WorldMap* _worldMap; int contador = 0; int estado = 0; - QVector2D calculaSemiCircle(const QVector2D& ballPosition, float radius); gk_control *_gk_control; + dk_control *_dk_control; + ak_control *_ak_control; private slots: diff --git a/src/entities/player/ak_control.cpp b/src/entities/player/ak_control.cpp new file mode 100644 index 0000000..54d559a --- /dev/null +++ b/src/entities/player/ak_control.cpp @@ -0,0 +1,26 @@ +#include "ak_control.h" +#define ROBOT_DIAMETER (2.0F * ROBOT_RADIUS) +#define BALL_RADIUS 0.0215 +#define BALL_DIAMETER (2.0F * BALL_RADIUS) +#define ROBOT_RADIUS 0.09 +#define BALL_SPEED 6.0 + +ak_control::ak_control(WorldMap *worldMap) { + _worldMap = worldMap; + _aPlayer = nullptr; +} + +void ak_control::setAPlayer (Player *player) +{ + _aPlayer = player; +} + +void ak_control::atackposition1() +{ + if(_aPlayer != nullptr){ + QVector2D ballPosition = _worldMap->ballPosition(); + const QVector2D pontoCerto(0.2f, 1.7f); + _aPlayer->goTo(pontoCerto); + _aPlayer->rotateTo(ballPosition); + } +} diff --git a/src/entities/player/ak_control.h b/src/entities/player/ak_control.h new file mode 100644 index 0000000..eb6cdfe --- /dev/null +++ b/src/entities/player/ak_control.h @@ -0,0 +1,27 @@ +#ifndef AK_CONTROL_H +#define AK_CONTROL_H + +#include +#include +#include +#include +#include +#include +#include + +class ak_control +{ +public: + ak_control(WorldMap *worldMap); + void setAPlayer(Player *player); + + void atackposition1(); + void atackposition2(); + + +private: + Player *_aPlayer; + WorldMap *_worldMap; +}; + +#endif // AK_CONTROL_H diff --git a/src/entities/player/dk_control.cpp b/src/entities/player/dk_control.cpp new file mode 100644 index 0000000..6036591 --- /dev/null +++ b/src/entities/player/dk_control.cpp @@ -0,0 +1,159 @@ +#include "dk_control.h" +#define ROBOT_DIAMETER (2.0F * ROBOT_RADIUS) +#define BALL_RADIUS 0.0215 +#define BALL_DIAMETER (2.0F * BALL_RADIUS) +#define ROBOT_RADIUS 0.09 +#define BALL_SPEED 6.0 + +dk_control::dk_control(WorldMap *worldMap) { + + _worldMap = worldMap; + _dPlayer = nullptr; +} + +void dk_control::setDPlayer (Player *player) +{ + _dPlayer = player; +} + +void dk_control::defensePass0(Player *atacplayer) +{ + //faz o passe para um player especifico + if(_dPlayer != nullptr){ + QVector2D ballPosition = _worldMap->ballPosition(); + QVector2D playerBLUE0Position = atacplayer -> getPosition(); + + QVector2D targetDirection = playerBLUE0Position - ballPosition; //calcula a direção da bola até o alvo + _dPlayer->rotateTo(playerBLUE0Position); + + float playerOrientation = _dPlayer->getOrientation(); + + float targetDirectionAngle = atan2(targetDirection.y(), targetDirection.x()); + + if(_dPlayer->getPosition().distanceToPoint(ballPosition) <= (ROBOT_RADIUS + BALL_RADIUS) && abs(playerOrientation - targetDirectionAngle) < 0.02){ + // Chuta a bola + _dPlayer->kick(4,false); + } + } +} + +void dk_control::intercept0(Player *Player3) +{ + if(_dPlayer != nullptr){ + + QVector2D ballPosition = _worldMap->ballPosition(); + _dPlayer->dribble(true); + //const QVector2D pontoCerto(-2.5f, 0.5f); + + if(ballPosition.x() <= 0.0f && ballPosition.y() >=0.0 && _dPlayer->getPosition().distanceToPoint(ballPosition)>(ROBOT_RADIUS + BALL_RADIUS)* 5){ + // obtém a posição do centro do gol + QVector2D goalCenter = _worldMap->ourGoalCenter(); + + // calcula o vetor do centro do gol até a bola + QVector2D toBall = ballPosition - goalCenter; + toBall.normalize(); + // multiplica o vetor por um valor para definir a distância da defesa à bola + QVector2D targetPosition = goalCenter + toBall * 2; + + // move a defesa para a posição de defesa + _dPlayer->goTo(targetPosition); + _dPlayer->rotateTo(ballPosition); + + } + if(_dPlayer->getPosition().distanceToPoint(ballPosition)<=(ROBOT_RADIUS + BALL_RADIUS) * 5 && _dPlayer->getPosition().distanceToPoint(ballPosition)>(ROBOT_RADIUS + BALL_RADIUS)){ + _dPlayer->goTo(ballPosition); + _dPlayer->rotateTo(ballPosition); + } + else{ + + if(_dPlayer->getPosition().distanceToPoint(ballPosition)<=(ROBOT_RADIUS + BALL_RADIUS)){ + QVector2D ballPosition = _worldMap->ballPosition(); + QVector2D playerBLUE0Position = Player3 -> getPosition(); + + QVector2D targetDirection = playerBLUE0Position - ballPosition; //calcula a direção da bola até o alvo + _dPlayer->rotateTo(playerBLUE0Position); + + float playerOrientation = _dPlayer->getOrientation(); + + float targetDirectionAngle = atan2(targetDirection.y(), targetDirection.x()); + + if(_dPlayer->getPosition().distanceToPoint(ballPosition) <= (ROBOT_RADIUS + BALL_RADIUS) && abs(playerOrientation - targetDirectionAngle) < 0.02){ + // Chuta a bola + _dPlayer->kick(5,false); + } + } + } + } +} + +void dk_control::intercept4(Player *Player2) +{ + if(_dPlayer != nullptr){ + + QVector2D ballPosition = _worldMap->ballPosition(); + _dPlayer->dribble(true); + //const QVector2D pontoCerto(-2.5f, 0.5f); + + if(ballPosition.x() <= 0.0f && ballPosition.y() <= 0.0 && _dPlayer->getPosition().distanceToPoint(ballPosition)>(ROBOT_RADIUS + BALL_RADIUS) *5 ) { + // obtém a posição do centro do gol + QVector2D goalCenter = _worldMap->ourGoalCenter(); + + // calcula o vetor do centro do gol até a bola + QVector2D toBall = ballPosition - goalCenter; + toBall.normalize(); + // multiplica o vetor por um valor para definir a distância da defesa à bola + QVector2D targetPosition = goalCenter + toBall * 2; + + // move a defesa para a posição de defesa + _dPlayer->goTo(targetPosition); + _dPlayer->rotateTo(ballPosition); + + } + if(_dPlayer->getPosition().distanceToPoint(ballPosition)<=(ROBOT_RADIUS + BALL_RADIUS) * 5 && _dPlayer->getPosition().distanceToPoint(ballPosition)>(ROBOT_RADIUS + BALL_RADIUS)){ + _dPlayer->goTo(ballPosition); + _dPlayer->rotateTo(ballPosition); + + } + else{ + + if(_dPlayer->getPosition().distanceToPoint(ballPosition)<=(ROBOT_RADIUS + BALL_RADIUS)){ + QVector2D ballPosition = _worldMap->ballPosition(); + QVector2D playerBLUE0Position = Player2 -> getPosition(); + + QVector2D targetDirection = playerBLUE0Position - ballPosition; //calcula a direção da bola até o alvo + _dPlayer->rotateTo(playerBLUE0Position); + + float playerOrientation = _dPlayer->getOrientation(); + + float targetDirectionAngle = atan2(targetDirection.y(), targetDirection.x()); + + if(_dPlayer->getPosition().distanceToPoint(ballPosition) <= (ROBOT_RADIUS + BALL_RADIUS) && abs(playerOrientation - targetDirectionAngle) < 0.02){ + // Chuta a bola + _dPlayer->kick(5,false); + } + } + } + } +} + +void dk_control::defensePosition0() +{ + if(_dPlayer != nullptr){ + QVector2D ballPosition = _worldMap->ballPosition(); + const QVector2D pontoCerto(-2.5f, 0.8f); + _dPlayer->goTo(pontoCerto); + _dPlayer->rotateTo(ballPosition); + } +} + +void dk_control::defensePosition4() +{ + if(_dPlayer != nullptr){ + QVector2D ballPosition = _worldMap->ballPosition(); + const QVector2D pontoCerto(-2.5f, -0.8f); + _dPlayer->goTo(pontoCerto); + _dPlayer->rotateTo(ballPosition); + } +} + + diff --git a/src/entities/player/dk_control.h b/src/entities/player/dk_control.h new file mode 100644 index 0000000..5200ab1 --- /dev/null +++ b/src/entities/player/dk_control.h @@ -0,0 +1,32 @@ +#ifndef DK_CONTROL_H +#define DK_CONTROL_H + +#include +#include +#include +#include +#include +#include +#include + +class dk_control +{ +public: + dk_control(WorldMap *worldMap); + + void setDPlayer(Player *player); + void intercept0(Player *Player3); + void defensePass0(Player *defensePlayer); + void defensePosition0(); + void intercept4(Player *Player2); + void defensePass4(Player *defensePlayer); + void defensePosition4(); + + +private: + Player *_dPlayer; + WorldMap *_worldMap; + +}; + +#endif // DK_CONTROL_H diff --git a/src/entities/player/player.h b/src/entities/player/player.h index e1239b5..bdd0bdf 100644 --- a/src/entities/player/player.h +++ b/src/entities/player/player.h @@ -93,7 +93,9 @@ 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;; + friend class gk_control; + friend class dk_control; + friend class ak_control; /*! * \brief Make this Player go to a given target position.