From f3393efd8b49044e766a3ba3b761ed8f0cdca6b2 Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Thu, 16 Jul 2026 12:57:02 -0700 Subject: [PATCH 1/5] make brake request and button binding --- src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/commands/ManualDriveCommand.java | 28 +++++++++++++++---- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1c1098..3e85dfc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -561,6 +561,7 @@ private void configureManualDriveBindings() { ); swerve.setDefaultCommand(manualDriveCommand); driver.back().onTrue(Commands.runOnce(() -> manualDriveCommand.seedFieldCentric())); + driver.povRight().onTrue(Commands.runOnce(() -> manualDriveCommand.toggleBrake())); } public static double ExponentialConvert(double controllerValue, double exponent) { diff --git a/src/main/java/frc/robot/commands/ManualDriveCommand.java b/src/main/java/frc/robot/commands/ManualDriveCommand.java index 749ef34..d82929c 100644 --- a/src/main/java/frc/robot/commands/ManualDriveCommand.java +++ b/src/main/java/frc/robot/commands/ManualDriveCommand.java @@ -32,7 +32,8 @@ public class ManualDriveCommand extends Command { private enum State { IDLING, DRIVING_WITH_MANUAL_ROTATION, - DRIVING_WITH_LOCKED_HEADING + DRIVING_WITH_LOCKED_HEADING, + BRAKE } private static final Time kHeadingLockDelay = Seconds.of(0.25); // time to wait before locking heading @@ -57,6 +58,9 @@ private enum State { private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withSteerRequestType(SteerRequestType.MotionMagicExpo); + + private final SwerveRequest.SwerveDriveBrake brakeRequest = new SwerveRequest.SwerveDriveBrake(); + private boolean braking = false; private State currentState = State.IDLING; private Optional lockedHeading = Optional.empty(); @@ -148,12 +152,18 @@ public void execute() { previousInput = input; return; } - if (input.hasRotation()) { + + if (braking) { + currentState = State.BRAKE; + } + else { + if (input.hasRotation()) { currentState = State.DRIVING_WITH_MANUAL_ROTATION; - } else if (input.hasTranslation()) { - currentState = lockedHeading.isPresent() ? State.DRIVING_WITH_LOCKED_HEADING : State.DRIVING_WITH_MANUAL_ROTATION; - } else if (previousInput.hasRotation() || previousInput.hasTranslation()) { - currentState = State.IDLING; + } else if (input.hasTranslation()) { + currentState = lockedHeading.isPresent() ? State.DRIVING_WITH_LOCKED_HEADING : State.DRIVING_WITH_MANUAL_ROTATION; + } else if (previousInput.hasRotation() || previousInput.hasTranslation()) { + currentState = State.IDLING; + } } previousInput = input; @@ -178,9 +188,15 @@ public void execute() { .withTargetDirection(lockedHeading.get()) ); break; + case BRAKE: + swerve.setControl(brakeRequest); } } + public void toggleBrake() { + braking = !braking; + } + @Override public boolean isFinished() { // Default drive command: runs until interrupted From 96c9d0481990f5f476d421da52eef2c136212937 Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Thu, 16 Jul 2026 16:18:22 -0700 Subject: [PATCH 2/5] remove conflict with sim --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3e85dfc..58d5d20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -447,9 +447,9 @@ private void configureSimBindings() { simButton(Constants.SimControllerButtons.kHangerUp) .or(driverPovLeft()) .onTrue(hanger.positionCommand(Hanger.Position.HANGER_EXTEND)); - simButton(Constants.SimControllerButtons.kHangerDown) - .or(driverPovRight()) - .onTrue(hanger.positionCommand(Hanger.Position.HANGER_HOME)); + // simButton(Constants.SimControllerButtons.kHangerDown) + // .or(driverPovRight()) + // .onTrue(hanger.positionCommand(Hanger.Position.HANGER_HOME)); simButton(Constants.SimControllerButtons.kHoodForward) .or(driver.y()) .onTrue(hood.positionCommand(0.75)); From 238102546f3785581163a53a239a91889c76cbdd Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Thu, 16 Jul 2026 17:09:50 -0700 Subject: [PATCH 3/5] move all braking logic to RobotContainer --- simgui-ds.json | 14 ++-- src/main/java/frc/robot/RobotContainer.java | 74 ++++++++++--------- .../robot/commands/ManualDriveCommand.java | 32 +++----- 3 files changed, 54 insertions(+), 66 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 8e88840..d22008a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -42,7 +47,7 @@ "key270": 324, "key315": 327, "key45": 329, - "key90": 326 + "key90": 54 } ], "povCount": 1 @@ -83,7 +88,7 @@ 51, 52, 53, - 54 + -1 ], "povCount": 0 }, @@ -94,11 +99,6 @@ } ], "robotJoysticks": [ - { - "guid": "78696e70757401000000000000000000", - "useGamepad": true - }, - {}, { "guid": "Keyboard0" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 58d5d20..9a51867 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -384,41 +384,43 @@ private void configureBindings() { } private void configureSimBindings() { - swerve.setDefaultCommand( - swerve.applyRequest(() -> { - if (!isSimControllerConnected() && !isDriverControllerConnected()) { - return fieldCentricDrive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0); - } - - double exponentVelocity = - Constants.SimConstants.controllerVelocityCurveExponent; - double exponentRotation = - Constants.SimConstants.controllerRotationCurveExponent; - - if (!simRobotCentricMode) { - double fieldX = fieldXSlewFilter.calculate( - Constants.Driving.kMaxSpeed.in(MetersPerSecond) - * ExponentialConvert(getSimLeftInput(), exponentVelocity)); - double fieldY = fieldYSlewFilter.calculate( - Constants.Driving.kMaxSpeed.in(MetersPerSecond) - * ExponentialConvert(getSimForwardInput(), exponentVelocity)); - double fieldRotate = fieldRotateSlewFilter.calculate( - Constants.Driving.kMaxRotationalRate.in(RadiansPerSecond) - * ExponentialConvert(getSimRotationInput(), exponentRotation)); - return fieldCentricDrive.withVelocityX(fieldX).withVelocityY(fieldY).withRotationalRate(fieldRotate); - } else { - double robotX = robotXSlewFilter.calculate( - Constants.Driving.kMaxSpeed.in(MetersPerSecond) - * ExponentialConvert(getSimLeftInput(), exponentVelocity)); - double robotY = robotYSlewFilter.calculate( - Constants.Driving.kMaxSpeed.in(MetersPerSecond) - * ExponentialConvert(getSimForwardInput(), exponentVelocity)); - double robotRotate = robotRotateSlewFilter.calculate( - Constants.Driving.kMaxRotationalRate.in(RadiansPerSecond) - * ExponentialConvert(getSimRotationInput(), exponentRotation)); - return robotCentricDrive.withVelocityX(robotX).withVelocityY(robotY).withRotationalRate(robotRotate); - } - })); + // swerve.setDefaultCommand( + // swerve.applyRequest(() -> { + // if (!isSimControllerConnected() && !isDriverControllerConnected()) { + // return fieldCentricDrive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0); + // } + + // double exponentVelocity = + // Constants.SimConstants.controllerVelocityCurveExponent; + // double exponentRotation = + // Constants.SimConstants.controllerRotationCurveExponent; + + // if (!simRobotCentricMode) { + // double fieldX = fieldXSlewFilter.calculate( + // Constants.Driving.kMaxSpeed.in(MetersPerSecond) + // * ExponentialConvert(getSimLeftInput(), exponentVelocity)); + // double fieldY = fieldYSlewFilter.calculate( + // Constants.Driving.kMaxSpeed.in(MetersPerSecond) + // * ExponentialConvert(getSimForwardInput(), exponentVelocity)); + // double fieldRotate = fieldRotateSlewFilter.calculate( + // Constants.Driving.kMaxRotationalRate.in(RadiansPerSecond) + // * ExponentialConvert(getSimRotationInput(), exponentRotation)); + // return fieldCentricDrive.withVelocityX(fieldX).withVelocityY(fieldY).withRotationalRate(fieldRotate); + // } else { + // double robotX = robotXSlewFilter.calculate( + // Constants.Driving.kMaxSpeed.in(MetersPerSecond) + // * ExponentialConvert(getSimLeftInput(), exponentVelocity)); + // double robotY = robotYSlewFilter.calculate( + // Constants.Driving.kMaxSpeed.in(MetersPerSecond) + // * ExponentialConvert(getSimForwardInput(), exponentVelocity)); + // double robotRotate = robotRotateSlewFilter.calculate( + // Constants.Driving.kMaxRotationalRate.in(RadiansPerSecond) + // * ExponentialConvert(getSimRotationInput(), exponentRotation)); + // return robotCentricDrive.withVelocityX(robotX).withVelocityY(robotY).withRotationalRate(robotRotate); + // } + // })); + configureManualDriveBindings(); + // Mirror driver-facing bindings on the sim joystick so the same features exist in sim. simButton(Constants.SimControllerButtons.kAutoAim) .or(driverRightStickButton()) @@ -561,7 +563,7 @@ private void configureManualDriveBindings() { ); swerve.setDefaultCommand(manualDriveCommand); driver.back().onTrue(Commands.runOnce(() -> manualDriveCommand.seedFieldCentric())); - driver.povRight().onTrue(Commands.runOnce(() -> manualDriveCommand.toggleBrake())); + driver.povRight().toggleOnTrue(swerve.applyRequest(() -> new SwerveRequest.SwerveDriveBrake())); } public static double ExponentialConvert(double controllerValue, double exponent) { diff --git a/src/main/java/frc/robot/commands/ManualDriveCommand.java b/src/main/java/frc/robot/commands/ManualDriveCommand.java index d82929c..1bdc40f 100644 --- a/src/main/java/frc/robot/commands/ManualDriveCommand.java +++ b/src/main/java/frc/robot/commands/ManualDriveCommand.java @@ -32,8 +32,7 @@ public class ManualDriveCommand extends Command { private enum State { IDLING, DRIVING_WITH_MANUAL_ROTATION, - DRIVING_WITH_LOCKED_HEADING, - BRAKE + DRIVING_WITH_LOCKED_HEADING } private static final Time kHeadingLockDelay = Seconds.of(0.25); // time to wait before locking heading @@ -58,9 +57,6 @@ private enum State { private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withSteerRequestType(SteerRequestType.MotionMagicExpo); - - private final SwerveRequest.SwerveDriveBrake brakeRequest = new SwerveRequest.SwerveDriveBrake(); - private boolean braking = false; private State currentState = State.IDLING; private Optional lockedHeading = Optional.empty(); @@ -153,20 +149,16 @@ public void execute() { return; } - if (braking) { - currentState = State.BRAKE; - } - else { - if (input.hasRotation()) { - currentState = State.DRIVING_WITH_MANUAL_ROTATION; - } else if (input.hasTranslation()) { - currentState = lockedHeading.isPresent() ? State.DRIVING_WITH_LOCKED_HEADING : State.DRIVING_WITH_MANUAL_ROTATION; - } else if (previousInput.hasRotation() || previousInput.hasTranslation()) { - currentState = State.IDLING; - } + currentState = State.IDLING; + if (input.hasRotation()) { + currentState = State.DRIVING_WITH_MANUAL_ROTATION; + } else if (input.hasTranslation()) { + currentState = lockedHeading.isPresent() ? State.DRIVING_WITH_LOCKED_HEADING : State.DRIVING_WITH_MANUAL_ROTATION; + } else if (previousInput.hasRotation() || previousInput.hasTranslation()) { + currentState = State.IDLING; } + previousInput = input; - switch (currentState) { case IDLING: swerve.setControl(idleRequest); @@ -188,15 +180,9 @@ public void execute() { .withTargetDirection(lockedHeading.get()) ); break; - case BRAKE: - swerve.setControl(brakeRequest); } } - public void toggleBrake() { - braking = !braking; - } - @Override public boolean isFinished() { // Default drive command: runs until interrupted From 05533538c1b32662068f9e842dc2f01489806f9f Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Thu, 16 Jul 2026 17:11:46 -0700 Subject: [PATCH 4/5] delete commented sim code block --- src/main/java/frc/robot/RobotContainer.java | 35 --------------------- 1 file changed, 35 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a51867..c022650 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -384,41 +384,6 @@ private void configureBindings() { } private void configureSimBindings() { - // swerve.setDefaultCommand( - // swerve.applyRequest(() -> { - // if (!isSimControllerConnected() && !isDriverControllerConnected()) { - // return fieldCentricDrive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0); - // } - - // double exponentVelocity = - // Constants.SimConstants.controllerVelocityCurveExponent; - // double exponentRotation = - // Constants.SimConstants.controllerRotationCurveExponent; - - // if (!simRobotCentricMode) { - // double fieldX = fieldXSlewFilter.calculate( - // Constants.Driving.kMaxSpeed.in(MetersPerSecond) - // * ExponentialConvert(getSimLeftInput(), exponentVelocity)); - // double fieldY = fieldYSlewFilter.calculate( - // Constants.Driving.kMaxSpeed.in(MetersPerSecond) - // * ExponentialConvert(getSimForwardInput(), exponentVelocity)); - // double fieldRotate = fieldRotateSlewFilter.calculate( - // Constants.Driving.kMaxRotationalRate.in(RadiansPerSecond) - // * ExponentialConvert(getSimRotationInput(), exponentRotation)); - // return fieldCentricDrive.withVelocityX(fieldX).withVelocityY(fieldY).withRotationalRate(fieldRotate); - // } else { - // double robotX = robotXSlewFilter.calculate( - // Constants.Driving.kMaxSpeed.in(MetersPerSecond) - // * ExponentialConvert(getSimLeftInput(), exponentVelocity)); - // double robotY = robotYSlewFilter.calculate( - // Constants.Driving.kMaxSpeed.in(MetersPerSecond) - // * ExponentialConvert(getSimForwardInput(), exponentVelocity)); - // double robotRotate = robotRotateSlewFilter.calculate( - // Constants.Driving.kMaxRotationalRate.in(RadiansPerSecond) - // * ExponentialConvert(getSimRotationInput(), exponentRotation)); - // return robotCentricDrive.withVelocityX(robotX).withVelocityY(robotY).withRotationalRate(robotRotate); - // } - // })); configureManualDriveBindings(); // Mirror driver-facing bindings on the sim joystick so the same features exist in sim. From afe76c7128def4db65a45a3b5e9fd4194a9c65fd Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Fri, 17 Jul 2026 10:01:04 -0700 Subject: [PATCH 5/5] Change structure of command, add log --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 13 ++++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2752346..8c6eb8a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -74,6 +74,8 @@ public void robotPeriodic() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); logPowerDistribution(); + + Logger.recordOutput("Drive/WheelsLocked", m_robotContainer.getBraking()); } private void logPowerDistribution() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c022650..5dc8752 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,6 +102,7 @@ public class RobotContainer { private final SlewRateLimiter robotYSlewFilter = new SlewRateLimiter(Constants.SlewLimits.slewTranslateLimit.in(MetersPerSecondPerSecond)); private final SlewRateLimiter robotRotateSlewFilter = new SlewRateLimiter(Constants.SlewLimits.slewRotateLimit.in(RadiansPerSecondPerSecond)); private boolean simRobotCentricMode = false; + private boolean braking = false; private final SubsystemCommands subsystemCommands = new SubsystemCommands( swerve, @@ -528,7 +529,13 @@ private void configureManualDriveBindings() { ); swerve.setDefaultCommand(manualDriveCommand); driver.back().onTrue(Commands.runOnce(() -> manualDriveCommand.seedFieldCentric())); - driver.povRight().toggleOnTrue(swerve.applyRequest(() -> new SwerveRequest.SwerveDriveBrake())); + + Command brakeCommand = swerve.applyRequest(() -> new SwerveRequest.SwerveDriveBrake()) + .beforeStarting(() -> braking = true) + .finallyDo((interrupted) -> braking = false); + + driver.povRight().toggleOnTrue(brakeCommand); + } public static double ExponentialConvert(double controllerValue, double exponent) { @@ -551,6 +558,10 @@ public void logSwerveStickyFaults() { swerve.logStickyFaults(); } + public boolean getBraking() { + return braking; + } + public void autonomousInit() { Logger.recordOutput("Auto/CurrentAuto", autoChooser.getSelected().getName()); }