Add aim lead

src/main/java/frc4388/robot/RobotContainer.java

@@ -39,6 +39,7 @@ import frc4388.robot.commands.MoveUntilSuply;
 // import frc4388.robot.commands.wait.waitFeedCoral;
 import frc4388.robot.commands.wait.waitSupplier;
 import frc4388.robot.constants.Constants;
+import frc4388.robot.constants.FieldConstants;
 import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.constants.Constants.OIConstants;
 import frc4388.robot.constants.Constants.SimConstants.Mode;
@@ -111,12 +112,9 @@ public class RobotContainer {
             // IF the driver is holding the aim button, aim the robot towards the hub
             if(m_driverXbox.getRightTriggerAxis() > 0.5) {
                 // Aim 
-                Translation2d shootTarget = new Translation2d();
-                // new Rotation2()
-                Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle();
-                m_robotSwerveDrive.driveFieldAngle(
+                m_robotSwerveDrive.driveFacingPosition(
                     getDeadbandedDriverController().getLeft(),
-                    ang);
+                    FieldConstants.BLUE_HUB_POS);
             } else {
                 // Drive normally
                 m_robotSwerveDrive.driveWithInput(

src/main/java/frc4388/robot/constants/Constants.java

@@ -92,19 +92,6 @@ public final class Constants {
         public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
     }
 
-    public static final class FieldConstants {
-        public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
-
-        // Test april tag field layout
-        // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
-        //     Arrays.asList(new AprilTag[] {
-        //         new AprilTag(1, new Pose3d(
-        //             new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
-        //         )),
-        //     }), 100, 100);
-
-    }
-
     public static final class LEDConstants {
         public static final int LED_SPARK_ID = 9;
 

src/main/java/frc4388/robot/constants/FieldConstants.java

@@ -0,0 +1,14 @@
+package frc4388.robot.constants;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public final class FieldConstants {
+    public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+    
+    public static final Translation2d BLUE_HUB_POS = new Translation2d();
+    public static final Translation2d RED_HUB_POS = new Translation2d();
+
+
+}

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -251,14 +251,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         io.setControl(ctrl);
     }
 
+    // Drive with a specific velocity and heading
     public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) {
         if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going:
             stopModules(); // stop the swerve
 
-        // if (leftStick.getNorm() < 0.05) // if no imput
-        //     return; // don't bother doing swerve drive math and return early.
-
-
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
 
@@ -268,13 +265,33 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             .withVelocityY(leftStick.getY() * speedAdjust)
             .withTargetDirection(heading);
         ctrl.HeadingController.setPID(
-            SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
-            SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
-            SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
+            SwerveDriveConstants.PIDConstants.AIM_kP.get(),
+            SwerveDriveConstants.PIDConstants.AIM_kI.get(),
+            SwerveDriveConstants.PIDConstants.AIM_kD.get()
+            // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP,
+            // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI,
+            // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD
         );
         io.setControl(ctrl);
         // SmartDashboard.putBoolean("drift correction", true);
-        
+    }
+
+    // Drive with the robot facing towards a specific position
+    public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
+
+        // Get the current speed of the robot
+        Translation2d robotSpeed = new Translation2d(
+            chassisSpeeds.vxMetersPerSecond, 
+            chassisSpeeds.vyMetersPerSecond
+        );
+
+        // Calculate a point to aim ahead of the actual position.
+        Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos);
+
+        // Calculate the angle between the current position and the lead position
+        Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
+
+        driveFieldAngle(leftStick, ang);
     }
 
     public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {

src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java

@@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Angle;
+import frc4388.utility.configurable.ConfigurableDouble;
 //import edu.wpi.first.units.measure.measure.Distance;
 import frc4388.utility.status.CanDevice;
 import frc4388.utility.structs.Gains;
@@ -37,6 +38,9 @@ public final class SwerveDriveConstants {
     public static       double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
     public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
 
+    // TODO: Replace with a constant
+    public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
+
     public static final double CORRECTION_MIN = 10;
     public static final double CORRECTION_MAX = 50;
     
@@ -156,6 +160,14 @@ public final class SwerveDriveConstants {
         
         public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
         public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
+
+
+
+        // TODO: Replace this with a static constant
+        public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 2.5);
+        public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0);
+        public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1);
+        // public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1);
     }
 
     public static final class Configurations {

src/main/java/frc4388/robot/subsystems/vision/VisionReal.java

@@ -8,7 +8,7 @@ import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
-import frc4388.robot.constants.Constants.FieldConstants;
+import frc4388.robot.constants.FieldConstants;
 import frc4388.robot.constants.Constants.VisionConstants;
 
 public class VisionReal implements VisionIO {