From ce68a721ac1bc3e4904c3be170599f4be6136255 Mon Sep 17 00:00:00 2001
From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com>
Date: Sat, 25 Jan 2025 14:57:34 -0500
Subject: [PATCH] started working on getting the robot to drive to the nearest
 target position

---
 .../java/frc/robot/DriveToPoseCommand.java    |  1 -
 src/main/java/frc/robot/Robot.java            | 27 ++++++++++++++-----
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/DriveToPoseCommand.java b/src/main/java/frc/robot/DriveToPoseCommand.java
index 79d2d90..54ea7fd 100644
--- a/src/main/java/frc/robot/DriveToPoseCommand.java
+++ b/src/main/java/frc/robot/DriveToPoseCommand.java
@@ -9,7 +9,6 @@
 import frc.robot.vision.Vision;
 
 public class DriveToPoseCommand extends Command {
-
     public final Drivetrain swerve;
     public final Vision vision;
     public final Pose2d targetPose;
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 308d3a3..edbe7ca 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -42,8 +42,6 @@ public class Robot extends TimedRobot {
   private final LEDs m_LEDs;
   private final Vision m_vision;
   private final Auto m_auto;
-  private final Pose2d m_targetPose;
-  private final DriveToPoseCommand moveToPoseCommand;
 
   // private final AutoFactory m_autoFactory;
 
@@ -69,10 +67,6 @@ public Robot() {
 
     m_vision = new Vision();
 
-    m_targetPose = new Pose2d(1.0, 4.0, Rotation2d.fromDegrees(0));
-
-    moveToPoseCommand = new DriveToPoseCommand(m_swerve, m_vision, m_targetPose);
-
     configureButtonBindings();
     configureEventBindings();
     configureAutoOptions();
@@ -185,7 +179,7 @@ private void configureDriverButtonBindings() {
         .ignoringDisable(true));
 
     m_driver.AIn()
-        .whileTrue(moveToPoseCommand);
+        .whileTrue(new DriveToPoseCommand(m_swerve, m_vision, getNearestPose(m_swerve.getPose(), targetPoses)));
 
   }
   // spotless:on
@@ -213,6 +207,25 @@ public double getPDHCurrent(int CANBusPort) {
     return m_powerDistribution.getCurrent(CANBusPort - 10);
   }
 
+  Pose2d[] targetPoses = {
+    new Pose2d(1.0, 3.0, Rotation2d.fromDegrees(0.0)),
+    new Pose2d(1.0, 5.0, Rotation2d.fromDegrees(0.0))
+  };
+
+  public Pose2d getNearestPose(Pose2d currentPose, Pose2d[] targetPoses) {
+    Pose2d closestPose = new Pose2d(5, 5, Rotation2d.fromDegrees(0));
+    double minDistance = Double.MAX_VALUE;
+
+    for (Pose2d targetPose : targetPoses) {
+      double distance = currentPose.getTranslation().getDistance(targetPose.getTranslation());
+
+      if (distance < minDistance) {
+        closestPose = targetPose;
+      }
+    }
+    return closestPose;
+  }
+
   private synchronized StructPublisher<Pose2d> getPose2dPublisher(String name) {
     var publisher = posePublishers.get(name);
     if (publisher == null) {