started working on getting the robot to drive to the nearest target p…

…osition

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;

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) {