Working on various auto lock on and some Auto stuff

src/main/java/frc/robot/RobotContainer.java

@@ -5,6 +5,10 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -33,7 +37,7 @@ public class RobotContainer {
 
     public RobotContainer() {
         vision = new PhotonVision();
-    drivetrain = new Drivetrain();
+        drivetrain = new Drivetrain(null);
 
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
 
@@ -43,6 +47,12 @@ public class RobotContainer {
         shiftTimer.reset();
 
         configureBindings();
+        configureShiftDisplay();
+
+        if(AutoConstants.kAutoConfigOk) {
+            autoChooser = AutoBuilder.buildAutoChooser();
+            configureNamedCommands();
+        }
     }
   
     private void configureBindings() {
@@ -51,23 +61,32 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-        () -> false
+                () -> true
             )
         );
 
-    driver.start().and(driver.x()).whileTrue(drivetrain.runFrontLeft(1, 0));
+        driver.a().whileTrue(
-    driver.start().and(driver.y()).whileTrue(drivetrain.runFrontRight(1, 0));
+            drivetrain.lockRotationToHub(
-    driver.start().and(driver.a()).whileTrue(drivetrain.runRearLeft(1, 0));
+                driver::getLeftY, 
-    driver.start().and(driver.b()).whileTrue(drivetrain.runRearRight(1, 0));
+                driver::getLeftX, 
-    driver.start().negate().and(driver.x()).whileTrue(drivetrain.runFrontLeft(0, 45));
+                false // TODO Should this be true by default?
-    driver.start().negate().and(driver.y()).whileTrue(drivetrain.runFrontRight(0, 45));
+            ) 
-    driver.start().negate().and(driver.a()).whileTrue(drivetrain.runRearLeft(0, 45));
+        );
-    driver.start().negate().and(driver.b()).whileTrue(drivetrain.runRearRight(0, 45));
+    }
-    driver.rightBumper().whileTrue(drivetrain.setX());
 
-    //drivetrain.setDefaultCommand(drivetrain.disableOutputs());
+    private void configureNamedCommands() {
+        NamedCommands.registerCommand(
+            "Drivetrain Set X", 
+            drivetrain.setX()
+        );
 
-    configureShiftDisplay();
+        NamedCommands.registerCommand(
+            "Drivetrain Face Hub", 
+            drivetrain.rotateToPose(
+                Utilities.getHubPose(),
+                false // TODO Should this be true by default?
+            )
+        );
     }
 
     public Command getAutonomousCommand() {

src/main/java/frc/robot/constants/AutoConstants.java

@@ -10,6 +10,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.path.PathConstraints;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 
 // TODO This is all hold over from 2025, does any of it need to change?
 public class AutoConstants {
@@ -23,6 +24,8 @@ public class AutoConstants {
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 
+    public static final double kYawPIDTolerance = Units.degreesToRadians(2);
+
     public static final double kAlignPXYController = 2;
     public static final double kAlignPThetaController = 5;
 

src/main/java/frc/robot/constants/CompetitionConstants.java

@@ -2,6 +2,9 @@ package frc.robot.constants;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 
 public class CompetitionConstants {
     // THIS SHOULD BE FALSE DURING COMPETITION PLAY
@@ -10,4 +13,17 @@ public class CompetitionConstants {
     public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(
         AprilTagFields.kDefaultField
     );
+
+    public static final Pose2d kBlueHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11), 
+        Units.inchesToMeters(158.84), 
+        Rotation2d.fromDegrees(0)
+    );
+
+    public static final Pose2d kRedHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11 + 143.5 * 2),
+        Units.inchesToMeters(158.84),
+        Rotation2d.fromDegrees(0)
+    );
+
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -22,10 +22,12 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
+import frc.robot.constants.CompetitionConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
@@ -41,10 +43,12 @@ public class Drivetrain extends SubsystemBase {
 
     private SwerveDrivePoseEstimator estimator;
 
+    private PIDController yawRotationController;
+
     private PhotonVision camera1;
     private PhotonVision camera2;
 
-    public Drivetrain() {
+    public Drivetrain(Pose2d startupPose) {
         frontLeft = new SwerveModule(
             "FrontLeft", 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
@@ -79,6 +83,14 @@ public class Drivetrain extends SubsystemBase {
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
+        yawRotationController = new PIDController(
+            AutoConstants.kPThetaController, 
+            0, 
+            0
+        );
+        yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
+        yawRotationController.setTolerance(AutoConstants.kYawPIDTolerance);
+
         // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
         estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics, 
@@ -89,7 +101,7 @@ public class Drivetrain extends SubsystemBase {
                 rearLeft.getPosition(),
                 rearRight.getPosition()
             },
-            new Pose2d()
+            startupPose != null ? startupPose : new Pose2d()
         );
 
         if(AutoConstants.kAutoConfigOk) {
@@ -131,7 +143,7 @@ public class Drivetrain extends SubsystemBase {
 
         Logger.recordOutput("Drivetrain/Pose", getPose());
         Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
-        Logger.recordOutput("Drivetrain/Heading", getHeading());
+        Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
     }
 
     public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
@@ -172,6 +184,66 @@ public class Drivetrain extends SubsystemBase {
         });  
     }
 
+    public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
+        return runOnce(yawRotationController::reset).andThen(
+            drive(
+                () -> 0,
+                () -> 0, 
+                () -> {
+                    Rotation2d targetRotation = new Rotation2d(
+                        targetPose.getX() - getPose().getX(), 
+                        targetPose.getY() - getPose().getY()
+                    );
+
+                    if(rotate180) {
+                        targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
+                    }
+
+                    return yawRotationController.calculate(
+                        getHeading().getRadians(),
+                        targetRotation.getRadians()
+                    );
+                }, 
+                () -> true
+            ))
+            .until(yawRotationController::atSetpoint);
+    }
+
+    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return runOnce(yawRotationController::reset).andThen(
+            drive(
+                xSpeed,
+                ySpeed, 
+                () -> {
+                    Optional<Alliance> alliance = DriverStation.getAlliance();
+
+                    if(alliance.isEmpty()) {
+                        return 0;
+                    }
+
+                    Pose2d faceTowards = alliance.get() == Alliance.Blue ?
+                        CompetitionConstants.kBlueHubLocation :
+                        CompetitionConstants.kRedHubLocation;
+
+                    Rotation2d targetRotation = new Rotation2d(
+                        faceTowards.getX() - getPose().getX(), 
+                        faceTowards.getY() - getPose().getY()
+                    );
+
+                    if(rotate180) {
+                        targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
+                    }
+
+                    return yawRotationController.calculate(
+                        getHeading().getRadians(),
+                        targetRotation.getRadians()
+                    );
+                }, 
+                () -> true
+            )
+        );
+    }
+
     // TODO check both cameras
     /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
         if (camera1 == null) {
@@ -314,7 +386,11 @@ public class Drivetrain extends SubsystemBase {
         return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
-    public double getHeading() {
+    public Rotation2d getHeading() {
+        return estimator.getEstimatedPosition().getRotation();
+    }
+
+    public double getHeadingDegrees() {
         return estimator.getEstimatedPosition().getRotation().getDegrees();
     }
 }

src/main/java/frc/robot/utilities/Utilities.java

@@ -1,7 +1,11 @@
 package frc.robot.utilities;
 
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.constants.CompetitionConstants;
 
 public class Utilities {
     public static final double kG = -9.81;
@@ -23,6 +27,16 @@ public class Utilities {
         return null;
     }
 
+    public static Pose2d getHubPose() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.Blue) {
+            return CompetitionConstants.kBlueHubLocation;
+        } else {
+            return CompetitionConstants.kRedHubLocation;
+        }
+    }
+
     /**
      * A ChatGPT possible hallucination related to calcuating whether a shot is possible
      * for a given speed and change in X and Y position