An idea for locking the drivetrain to X when not moving in auto lock on

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

@@ -148,7 +148,8 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
 
@@ -276,7 +277,8 @@ public class RobotContainer {
                 driver::getLeftY,
                 driver::getLeftX, 
                 driver::getRightX, 
-                () -> true
+                () -> true,
+                () -> false
             )
         );
         shooter.setDefaultCommand(shooter.stop());
@@ -295,7 +297,8 @@ public class RobotContainer {
             drivetrain.lockRotationToHub(
                 driver::getLeftY, 
                 driver::getLeftX, 
-                false
+                false,
+                true
             ) 
         );
 
@@ -447,7 +450,7 @@ public class RobotContainer {
                 .alongWith(
                     shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
                     intakePivot.jimmy(0.5),
-                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false))
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true))
                 .withTimeout(0.5));
 
         NamedCommands.registerCommand("auto shoot", 
@@ -456,7 +459,7 @@ public class RobotContainer {
                     shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
                     spindexer.spinToShooter(), 
                     intakePivot.jimmy(0.5),
-                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false)));
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true)));
     }
 
     public Command getAutonomousCommand() {

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

@@ -199,7 +199,7 @@ public class Drivetrain extends SubsystemBase {
      * @return A complete Command structure that performs the specified action
      */
     public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
-        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180)
+        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180, false)
             .until(yawRotationController::atSetpoint);
     }
 
@@ -220,12 +220,13 @@ public class Drivetrain extends SubsystemBase {
      *   of the robot faces the hub
      * @return A complete Command structure that performs the specified action
      */
-    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180, boolean lockXWhenStopped) {
         return lockRotationToSuppliedPose(
             Utilities::getHubPose, 
             xSpeed, 
             ySpeed, 
-            rotate180
+            rotate180,
+            lockXWhenStopped
         );
     }
 
@@ -245,7 +246,7 @@ public class Drivetrain extends SubsystemBase {
      *   of the robot faces the supplied pose
      * @return A complete Command structure that performs the specified action
      */
-    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180, boolean lockXWhenStopped) {
         return runOnce(yawRotationController::reset).andThen(
             drive(
                 xSpeed,
@@ -274,7 +275,8 @@ public class Drivetrain extends SubsystemBase {
 
                     return outputPower;
                 }, 
-                () -> true
+                () -> true,
+                () -> lockXWhenStopped
             )
         );
     }
@@ -290,13 +292,14 @@ public class Drivetrain extends SubsystemBase {
      * @param ySpeed The Y (left/right) translational speed of the robot
      * @return A complete Command structure that performs the specified action
      */
-    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
+    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean lockXWhenStopped) {
         return runOnce(yawRotationController::reset).andThen(
             drive(
                 xSpeed,
                 ySpeed, 
                 () -> yawRotationController.calculate(yaw.getAsDouble(), 0),
-                () -> true
+                () -> true,
+                () -> lockXWhenStopped
             )
         );
     }
@@ -310,14 +313,14 @@ public class Drivetrain extends SubsystemBase {
         
     }
 
-    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
+    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative, BooleanSupplier lockXWhenStopped) {
-        // TODO Specific Alliance code?
         return run(() -> {
             drive(
                 -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
                 -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
                 -MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
-                fieldRelative.getAsBoolean()
+                fieldRelative.getAsBoolean(),
+                lockXWhenStopped.getAsBoolean()
             );
         });
     }
@@ -369,30 +372,41 @@ public class Drivetrain extends SubsystemBase {
         );
     }
 
-    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
+    public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative, boolean lockXWhenStopped) {
-        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
+        if(lockXWhenStopped && 
-        double xSpeedDelivered = 0;
+            MathUtil.applyDeadband(xSpeed, OIConstants.kDriveDeadband) == 0 &&
-        double ySpeedDelivered = 0;
+            MathUtil.applyDeadband(ySpeed, OIConstants.kDriveDeadband) == 0 &&
+            MathUtil.applyDeadband(rotation, OIConstants.kDriveDeadband) == 0) {
 
-        if(p != 0){
+            frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
-            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-        }else{
+            rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
-            xSpeedDelivered = 0;
+        } else {
-            ySpeedDelivered = 0;
+            double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
+            double xSpeedDelivered = 0;
+            double ySpeedDelivered = 0;
+
+            if(p != 0){
+                xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+                ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            }else{
+                xSpeedDelivered = 0;
+                ySpeedDelivered = 0;
+            }
+
+            double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
+
+            SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
+                fieldRelative ?
+                    ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
+                        //estimator.getEstimatedPosition().getRotation()) :
+                        Rotation2d.fromDegrees(getGyroValue())) :
+                    new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
+            );
+
+            setModuleStates(swerveModuleStates);
         }
-
-        double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
-
-        SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
-            fieldRelative ?
-                ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
-                    //estimator.getEstimatedPosition().getRotation()) :
-                    Rotation2d.fromDegrees(getGyroValue())) :
-                new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
-        );
-
-        setModuleStates(swerveModuleStates);
     }
 
     public void driveWithChassisSpeeds(ChassisSpeeds speeds) {