From 2010404aa2c8d5fd742201b6fc93c14d85fa7947 Mon Sep 17 00:00:00 2001
From: HeitorCordeiro <heitor.cordeiro99@gmail.com>
Date: Fri, 14 Mar 2025 22:09:28 -0300
Subject: [PATCH] =?UTF-8?q?Implementa=C3=A7=C3=A3o=20do=20ball=20placement?=
 =?UTF-8?q?=20simplificado?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../Processing/CustomPlayer/CustomPlayer.cpp  | 52 +++++++++++++++++--
 .../Processing/CustomPlayer/CustomPlayer.h    |  3 ++
 2 files changed, 52 insertions(+), 3 deletions(-)

diff --git a/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp b/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp
index 0232b9c..ab2c6cd 100644
--- a/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp
+++ b/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp
@@ -34,12 +34,58 @@ void CustomPlayer::update() {
 }
 
 void CustomPlayer::exec() {
-  if (!field || !frame || !robot) {
+
+  if (!field || !frame || !robot)
+    return;
+  if (!frame->has_ball())
     return;
+  auto&& pos_ball = frame->ball();
+
+  QPointF targetPosition(1000, 4000);
+  QPointF initialPosition(2000, 0);
+  int tolerance = 150;
+  maxdis = 9000000;
+  if ((pos_ball.distTo(targetPosition)) > tolerance) {
+
+    for (const auto& ally : frame->allies()) {
+      double distance = ally.distTo(pos_ball);
+      if (maxdis > distance) {
+        maxdis = distance;
+        idrobot = ally.id();
+      }
+    }
   }
 
-  // TODO: here...
-  // emit sendCommand(...);
+  if (robot->id() != idrobot)
+    return;
+
+  if ((pos_ball - targetPosition).manhattanLength() > tolerance) {
+    if (robot->distTo(pos_ball) > 150) {
+      SSLMotion::GoToPoint goToPoint(pos_ball, (pos_ball - robot->position()).angle(), true);
+      auto command = sslNavigation.run(robot.value(), SSLRobotCommand(goToPoint));
+      command.set_dribbler(true);
+      command.set_dribblerVelocity(4);
+      emit sendCommand(command);
+    } else {
+      SSLMotion::GoToPoint goToPoint(targetPosition,
+                                     (targetPosition - robot->position()).angle(),
+                                     true);
+      goToPoint.set_maxVelocity(1);
+      auto command = sslNavigation.run(robot.value(), SSLRobotCommand(goToPoint));
+      command.set_dribbler(true);
+      command.set_dribblerVelocity(4);
+      emit sendCommand(command);
+    }
+  } else if (robot->id() == idrobot && robot->distTo(initialPosition) > tolerance) {
+    SSLMotion::GoToPoint goToInitial(initialPosition,
+                                     (initialPosition - robot->position()).angle(),
+                                     true);
+    auto command = sslNavigation.run(robot.value(), SSLRobotCommand(goToInitial));
+    emit sendCommand(command);
+  } else {
+    idrobot = -1;
+    maxdis = 9000000;
+  }
 }
 
 void CustomPlayer::receiveField(const Field& field) {
diff --git a/src/Modules/Processing/CustomPlayer/CustomPlayer.h b/src/Modules/Processing/CustomPlayer/CustomPlayer.h
index 3aff99e..c7a12f7 100644
--- a/src/Modules/Processing/CustomPlayer/CustomPlayer.h
+++ b/src/Modules/Processing/CustomPlayer/CustomPlayer.h
@@ -5,6 +5,9 @@
 #include "Modules/Processing/ProcessingUtils/ProcessingUtils.h"
 
 class CustomPlayer : public Processing {
+  int maxdis = 9000000;
+  int idrobot = -1;
+  bool ball_placement = true;
  public:
   CustomPlayer(int index, QThreadPool* threadPool);