From 00bb1e7011c3a14eb42257555c1f61580edb1fea Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Wed, 1 Apr 2026 20:35:50 -0400 Subject: [PATCH] An idea for locking the drivetrain to X when not moving in auto lock on --- src/main/java/frc/robot/RobotContainer.java | 13 ++-- .../java/frc/robot/subsystems/Drivetrain.java | 78 +++++++++++-------- 2 files changed, 54 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6144c5c..3d5d501 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 4d22935..929428c 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/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 poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) { + public Command lockRotationToSuppliedPose(Supplier 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) { - // TODO Specific Alliance code? + public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative, BooleanSupplier lockXWhenStopped) { 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) { - double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); - double xSpeedDelivered = 0; - double ySpeedDelivered = 0; + public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative, boolean lockXWhenStopped) { + if(lockXWhenStopped && + MathUtil.applyDeadband(xSpeed, OIConstants.kDriveDeadband) == 0 && + MathUtil.applyDeadband(ySpeed, OIConstants.kDriveDeadband) == 0 && + MathUtil.applyDeadband(rotation, OIConstants.kDriveDeadband) == 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; + frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); + frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); + rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); + rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); + } else { + 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) {