From 6468c4915577c98af4d7cd4c540cfa21c2d04fd9 Mon Sep 17 00:00:00 2001
From: Stozux <jpds@cin.ufpe.br>
Date: Thu, 13 Mar 2025 22:33:52 -0300
Subject: [PATCH] penalti com toque

---
 .../Processing/CustomPlayer/CustomPlayer.cpp  | 38 ++++++++++++++++++-
 1 file changed, 36 insertions(+), 2 deletions(-)

diff --git a/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp b/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp
index 0232b9c..9517520 100644
--- a/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp
+++ b/src/Modules/Processing/CustomPlayer/CustomPlayer.cpp
@@ -1,4 +1,5 @@
 #include "CustomPlayer.h"
+#include "Packages/SSLRobotCommand/SSLRobotCommand.h"
 
 CustomPlayer::CustomPlayer(int index, QThreadPool* threadPool) : Processing(index, threadPool) {
 }
@@ -37,9 +38,42 @@ void CustomPlayer::exec() {
   if (!field || !frame || !robot) {
     return;
   }
+  if(!frame->has_ball()) return;
+  auto&& pos_ball = frame->ball();
+  if(field->enemyPenaltyAreaContains(pos_ball)) return;
+  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::RotateOnSelf Rotate((field->enemyGoalInsideCenter() -robot->position()).angle());
+    auto command = sslNavigation.run(robot.value(), SSLRobotCommand(Rotate));
+    command.set_dribbler(true);
+    command.set_dribblerVelocity(4);
+    emit sendCommand(command);
+    if(abs(robot->angleTo(field->enemyGoalInsideCenter()))<0.1)
+    {
+      SSLMotion::GoToPoint goToPoint(pos_ball,
+        (pos_ball - robot->position()).angle(),
+        true);
+      auto irparapos = sslNavigation.run(robot.value(), SSLRobotCommand(goToPoint));
+      irparapos.set_front(true);
+      irparapos.set_kickSpeed(2);
+
+      if(robot->distTo(field->enemyGoalInsideCenter())<4000)
+      {
+        irparapos.set_kickSpeed(6);
+      }
+      emit sendCommand(irparapos);
+    }
+
+  }
 
-  // TODO: here...
-  // emit sendCommand(...);
 }
 
 void CustomPlayer::receiveField(const Field& field) {