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;
@@ -22,110 +26,125 @@ import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
-  private PhotonVision vision;
+    private PhotonVision vision;
-  private Drivetrain drivetrain;
+    private Drivetrain drivetrain;
 
-  private CommandXboxController driver;
+    private CommandXboxController driver;
 
-  private SendableChooser<Command> autoChooser;
+    private SendableChooser<Command> autoChooser;
 
-  private Timer shiftTimer;
+    private Timer shiftTimer;
 
-  public RobotContainer() {
+    public RobotContainer() {
-    vision = new PhotonVision();
+        vision = new PhotonVision();
-    drivetrain = new Drivetrain();
+        drivetrain = new Drivetrain(null);
 
-    vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+        vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
 
-    driver = new CommandXboxController(OIConstants.kDriverControllerPort);
+        driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 
-    shiftTimer = new Timer();
+        shiftTimer = new Timer();
-    shiftTimer.reset();
+        shiftTimer.reset();
 
-    configureBindings();
+        configureBindings();
-  }
+        configureShiftDisplay();
 
-  private void configureBindings() {
+        if(AutoConstants.kAutoConfigOk) {
-    drivetrain.setDefaultCommand(
+            autoChooser = AutoBuilder.buildAutoChooser();
-      drivetrain.drive(
+            configureNamedCommands();
-        driver::getLeftY,
+        }
-        driver::getLeftX, 
-        driver::getRightX, 
-        () -> false
-      )
-    );
-
-    driver.start().and(driver.x()).whileTrue(drivetrain.runFrontLeft(1, 0));
-    driver.start().and(driver.y()).whileTrue(drivetrain.runFrontRight(1, 0));
-    driver.start().and(driver.a()).whileTrue(drivetrain.runRearLeft(1, 0));
-    driver.start().and(driver.b()).whileTrue(drivetrain.runRearRight(1, 0));
-    driver.start().negate().and(driver.x()).whileTrue(drivetrain.runFrontLeft(0, 45));
-    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());
-
-    configureShiftDisplay();
-  }
-
-  public Command getAutonomousCommand() {
-    if(AutoConstants.kAutoConfigOk) {
-      return autoChooser.getSelected();
-    } else {
-      return new PrintCommand("Robot Config loading failed, autonomous disabled");
     }
-  }
   
-  private void configureShiftDisplay() {
+    private void configureBindings() {
-    SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        drivetrain.setDefaultCommand(
+            drivetrain.drive(
+                driver::getLeftY,
+                driver::getLeftX, 
+                driver::getRightX, 
+                () -> true
+            )
+        );
 
-    RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
+        driver.a().whileTrue(
-      shiftTimer.stop();
+            drivetrain.lockRotationToHub(
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+                driver::getLeftY, 
-    }));
+                driver::getLeftX, 
+                false // TODO Should this be true by default?
+            ) 
+        );
+    }
 
-    RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
+    private void configureNamedCommands() {
-      Elastic.selectTab(OIConstants.kTeleopTab);
+        NamedCommands.registerCommand(
-      shiftTimer.reset();
+            "Drivetrain Set X", 
-      shiftTimer.start();
+            drivetrain.setX()
-    }));
+        );
 
-    new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
+        NamedCommands.registerCommand(
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+            "Drivetrain Face Hub", 
-    }));
+            drivetrain.rotateToPose(
+                Utilities.getHubPose(),
+                false // TODO Should this be true by default?
+            )
+        );
+    }
 
-    new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
+    public Command getAutonomousCommand() {
-      SmartDashboard.putStringArray(
+        if(AutoConstants.kAutoConfigOk) {
-        OIConstants.kCurrentActiveHub, 
+            return autoChooser.getSelected();
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+        } else {
-      );
+            return new PrintCommand("Robot Config loading failed, autonomous disabled");
-    }));
+        }
+    }
 
-    new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
+    private void configureShiftDisplay() {
-      SmartDashboard.putStringArray(
+        SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
-        OIConstants.kCurrentActiveHub, 
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
-      );
-    }));
         
-    new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
+        RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(
+            shiftTimer.stop();
-        OIConstants.kCurrentActiveHub, 
+            SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+        }));
-      );
-    }));
 
-    new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
+        RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(
+            Elastic.selectTab(OIConstants.kTeleopTab);
-        OIConstants.kCurrentActiveHub, 
+            shiftTimer.reset();
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+            shiftTimer.start();
-      );
+        }));
-    }));
 
-    new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
+        new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+            SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
-    }));
+        }));
-  }
+
+        new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        }));
+    }
 }

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