From 70a7d2010ad31e43fa888614f45d6de888a07d56 Mon Sep 17 00:00:00 2001 From: Zach Rutman Date: Tue, 2 Apr 2024 15:28:40 -0700 Subject: [PATCH] fix brake mode (#43) * feat: add usable brake mode logic * fix: remove enabledInit, add FMS check for isPreMatch --- .../java/org/team1540/robot2024/Robot.java | 30 ++++----- .../team1540/robot2024/RobotContainer.java | 61 ++++++++++++++----- .../robot2024/subsystems/indexer/Indexer.java | 4 +- .../subsystems/indexer/IndexerIO.java | 2 +- .../subsystems/indexer/IndexerIOSparkMax.java | 2 +- .../subsystems/indexer/IndexerIOTalonFX.java | 2 +- .../robot2024/subsystems/led/Leds.java | 14 +---- 7 files changed, 65 insertions(+), 50 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index fb4843c9..8b510894 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -98,8 +98,7 @@ public void robotInit() { // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); - robotContainer.shooter.setPivotBrakeMode(false); - robotContainer.drivetrain.setBrakeMode(false); + robotContainer.disableBrakeMode(); // Pathplanner warmup (helps prevents delays at the start of auto) FollowPathCommand.warmupCommand().schedule(); @@ -134,9 +133,6 @@ public void robotPeriodic() { */ @Override public void disabledInit() { -// robotContainer.elevator.setBrakeMode(false); - robotContainer.shooter.setPivotBrakeMode(false); - robotContainer.indexer.setIntakeNeutralMode(false); robotContainer.drivetrain.unblockTags(); robotContainer.shooter.setPivotPosition(new Rotation2d()); robotContainer.leds.setPattern(Leds.Zone.MAIN, new LedPatternRainbow(1)); @@ -151,19 +147,11 @@ public void disabledPeriodic() { AutoManager.getInstance().updateSelected(); } - public void enabledInit() { - robotContainer.elevator.setBrakeMode(true); - robotContainer.shooter.setPivotBrakeMode(true); - robotContainer.drivetrain.setBrakeMode(true); - robotContainer.indexer.setIntakeNeutralMode(true); - } - /** * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - // robotContainer.leds.setPatternAll(LedPatternFlame::new, Leds.PatternCriticality.HIGH); robotContainer.drivetrain.blockTags(); @@ -176,6 +164,11 @@ public void autonomousInit() { } + @Override + public void autonomousExit() { + robotContainer.enableBrakeMode(true); + } + /** * This function is called periodically during autonomous. */ @@ -183,12 +176,14 @@ public void autonomousInit() { public void autonomousPeriodic() { } + + /** * This function is called once when teleop is enabled. */ @Override public void teleopInit() { - enabledInit(); + robotContainer.enableBrakeMode(false); // robotContainer.leds.setPatternAll(() -> new LedPatternRainbow(2), Leds.PatternCriticality.HIGH); robotContainer.drivetrain.zeroFieldOrientation();// TODO: remove this once odometry / startup zero is good @@ -197,6 +192,10 @@ public void teleopInit() { } } + @Override + public void teleopExit() { + robotContainer.enableBrakeMode(true); + } /** * This function is called periodically during operator control. */ @@ -209,9 +208,6 @@ public void teleopPeriodic() { */ @Override public void testInit() { - robotContainer.leds.setPattern(Leds.Zone.MAIN,new LedPatternTuneColor()); - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index f2c4fb5e..448e7be7 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -4,12 +4,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.autos.*; import org.team1540.robot2024.commands.climb.ClimbAlignment; @@ -27,10 +26,7 @@ import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.led.Leds; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternProgressBar; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState; -import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave; -import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern; +import org.team1540.robot2024.subsystems.led.patterns.*; import org.team1540.robot2024.subsystems.shooter.Shooter; import org.team1540.robot2024.subsystems.tramp.Tramp; import org.team1540.robot2024.subsystems.vision.AprilTagVision; @@ -39,6 +35,8 @@ import org.team1540.robot2024.util.auto.AutoCommand; import org.team1540.robot2024.util.auto.AutoManager; +import java.util.function.BooleanSupplier; + import static org.team1540.robot2024.Constants.SwerveConfig; import static org.team1540.robot2024.Constants.isTuningMode; @@ -58,6 +56,9 @@ public class RobotContainer { public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS); + + + public boolean isBrakeMode; /** * The container for the robot. Contains subsystems, IO devices, and commands. */ @@ -130,7 +131,7 @@ private void configureButtonBindings() { driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); driver.y().onTrue(Commands.runOnce(() -> { drivetrain.zeroFieldOrientationManual(); - drivetrain.setBrakeMode(true); + enableBrakeMode(false); }).ignoringDisable(true)); Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter) @@ -214,17 +215,24 @@ private void configureButtonBindings() { new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4)); - new Trigger(RobotController::getUserButton).toggleOnTrue(Commands.startEnd( - () -> { - elevator.setBrakeMode(false); - leds.setPatternAll(() -> new LedPatternRSLState(Color.kMagenta), Leds.PatternLevel.ELEVATOR_STATE); - }, + + BooleanSupplier isPreMatch = () -> (!DriverStation.isDSAttached() || !DriverStation.isFMSAttached() || DriverStation.isAutonomous()) && DriverStation.isDisabled(); + Command brakeModeCommand = Commands.runOnce( () -> { - elevator.setBrakeMode(true); - leds.clearPatternAll(Leds.PatternLevel.ELEVATOR_STATE); + if (!isPreMatch.getAsBoolean()) {return;} + if (!isBrakeMode) { + enableBrakeMode(true); + } else { + disableBrakeMode(); + } } - ).ignoringDisable(true)); + ).ignoringDisable(true); + + new Trigger(tramp::isNoteStaged).debounce(0.1) + .onTrue(brakeModeCommand); + driver.start().onTrue(brakeModeCommand); + copilot.start().onTrue(brakeModeCommand); } @@ -286,4 +294,27 @@ private void configureAutoRoutines() { public Command getAutonomousCommand() { return AutoManager.getInstance().getSelected(); } + + public void enableBrakeMode(boolean requireRobotDisabled) { + if (!isBrakeMode && (!requireRobotDisabled || !DriverStation.isEnabled())) { + shooter.setPivotBrakeMode(true); + indexer.setIntakeBrakeMode(true); + elevator.setBrakeMode(true); + drivetrain.setBrakeMode(true); + leds.clearPattern(Leds.Zone.MAIN, Leds.PatternLevel.COAST_STATE); + isBrakeMode = true; + Logger.recordOutput("brakeMode", true); + } + } + public void disableBrakeMode() { + if (isBrakeMode && !DriverStation.isEnabled()) { + shooter.setPivotBrakeMode(false); + indexer.setIntakeBrakeMode(false); + elevator.setBrakeMode(false); + drivetrain.setBrakeMode(false); + leds.setPattern(Leds.Zone.MAIN, new LedPatternBreathing("#f000c7"), Leds.PatternLevel.COAST_STATE); + isBrakeMode = false; + Logger.recordOutput("brakeMode", false); + } + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index cf17f292..0acdb199 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -133,8 +133,8 @@ public Command commandRunIntake(double percent) { ); } - public void setIntakeNeutralMode(boolean isBrake) { - io.setIntakeNeutralMode(isBrake); + public void setIntakeBrakeMode(boolean isBrake) { + io.setIntakeBrakeMode(isBrake); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index 56d0842c..3d1b8ef7 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -30,6 +30,6 @@ default void setFeederVelocity(double velocityRPM) {} default void configureFeederPID(double p, double i, double d) {} - default void setIntakeNeutralMode(boolean isBrakeMode) {} + default void setIntakeBrakeMode(boolean isBrakeMode) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index d00087c5..df790fe4 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -74,7 +74,7 @@ public void configureFeederPID(double p, double i, double d) { feederPID.setD(d); } - public void setIntakeNeutralMode(boolean isBrakeMode) { + public void setIntakeBrakeMode(boolean isBrakeMode) { intakeMotor.setIdleMode(isBrakeMode ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java index cabb1293..fcac060d 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java @@ -116,7 +116,7 @@ public void configureFeederPID(double p, double i, double d) { } @Override - public void setIntakeNeutralMode(boolean isBrakeMode) { + public void setIntakeBrakeMode(boolean isBrakeMode) { intakeMotor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java index b85c6502..463cf3da 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java @@ -62,19 +62,7 @@ public void clearPattern(Zone zone, PatternLevel priority) { patterns[zone.ordinal()].clearPattern(priority); } - public void setPatternAll(Supplier patternSupplier, PatternLevel priority) { - for (int i = 0; i