Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Bradley Bickford
2026-04-01 20:35:50 -04:00
parent 07656eedc1
commit 00bb1e7011
2 changed files with 54 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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() {

36
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@@ -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) {
// 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,7 +372,17 @@ 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) {
if(lockXWhenStopped &&
MathUtil.applyDeadband(xSpeed, OIConstants.kDriveDeadband) == 0 &&
MathUtil.applyDeadband(ySpeed, OIConstants.kDriveDeadband) == 0 &&
MathUtil.applyDeadband(rotation, OIConstants.kDriveDeadband) == 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;
@@ -394,6 +407,7 @@ public class Drivetrain extends SubsystemBase {
setModuleStates(swerveModuleStates);
}
}
public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);