added a DriveToPoseCommand

src/main/java/frc/robot/Constants.java

@@ -102,6 +102,15 @@ public static final class TurningControllerGains {
       public static final double kD = 0.0; // 2023 Competition Robot
     }
 
+    public static final class DriveToPoseControllerGains {
+      public static final double kTraP = 1.0;
+      public static final double kTraI = 0.0;
+      public static final double kTraD = 0.0;
+      public static final double kRotP = 1.5;
+      public static final double kRotI = 0.0;
+      public static final double kRotD = 0.0;
+    }
+
     // Not adjusted
     // public static final double kMaxModuleAngularSpeedRadiansPerSecond = 0.05 * Math.PI;
     // public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared =

src/main/java/frc/robot/DriveToPoseCommand.java

@@ -0,0 +1,71 @@
+package frc.robot;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ModuleConstants.DriveToPoseControllerGains;
+import frc.robot.drivetrain.Drivetrain;
+import frc.robot.vision.Vision;
+
+public class DriveToPoseCommand extends Command {
+
+    public final Drivetrain swerve;
+    public final Vision vision;
+    public final Pose2d targetPose;
+
+    private final PIDController xController = new PIDController(
+        DriveToPoseControllerGains.kTraP, 
+        DriveToPoseControllerGains.kTraI, 
+        DriveToPoseControllerGains.kTraD);
+    private final PIDController yController = new PIDController(
+        DriveToPoseControllerGains.kTraP, 
+        DriveToPoseControllerGains.kTraI, 
+        DriveToPoseControllerGains.kTraD);
+    private final PIDController thetaController = new PIDController(
+        DriveToPoseControllerGains.kRotP, 
+        DriveToPoseControllerGains.kRotI, 
+        DriveToPoseControllerGains.kRotD);
+
+    public DriveToPoseCommand(Drivetrain drivetrain, Vision poseEstimator, Pose2d targetPosition) {
+        swerve = drivetrain;
+        vision = poseEstimator;
+        targetPose = targetPosition;
+
+        thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+        addRequirements(swerve);
+    }
+
+    @Override
+    public void initialize() {}
+
+    @Override
+    public void execute() {
+        Pose2d currentPose = swerve.getPose();
+
+        ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(
+            xController.calculate(currentPose.getX(), targetPose.getX()),
+            yController.calculate(currentPose.getY(), targetPose.getY()),
+            thetaController.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians())),
+            swerve.getHeading());
+
+        swerve.setChassisSpeeds(speeds);
+    }
+
+    @Override
+    public boolean isFinished() {
+        Pose2d currentPose = swerve.getPose();
+        double xError = Math.abs(targetPose.getX() - currentPose.getX());
+        double yError = Math.abs(targetPose.getY() - currentPose.getY());
+        double thetaError = Math.abs(targetPose.getRotation().getRadians() - currentPose.getRotation().getRadians());
+
+        return xError < 0.1 && yError < 0.1 && thetaError < Math.toRadians(3);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        ChassisSpeeds chassisSpeeds = new ChassisSpeeds(0, 0, 0);
+        swerve.setChassisSpeeds(chassisSpeeds);
+    }
+}

src/main/java/frc/robot/Robot.java

@@ -1,6 +1,7 @@
 package frc.robot;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -41,6 +42,8 @@ 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;
 
@@ -66,6 +69,10 @@ 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();
@@ -177,6 +184,12 @@ private void configureDriverButtonBindings() {
         .onTrue(new InstantCommand(() -> m_swerve.setHeadingOffset())
         .ignoringDisable(true));
 
+    m_driver.AIn()
+        .onTrue(moveToPoseCommand);
+
+    m_driver.HIn()
+        .onTrue(new InstantCommand(() -> CommandScheduler.getInstance().cancel(moveToPoseCommand)));
+
   }
   // spotless:on