diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java index 15dd98e..c0e28b0 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/lights/LightsSubsystem.java @@ -1,5 +1,6 @@ package com.swrobotics.robot.subsystems.lights; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.swrobotics.mathlib.MathUtil; import com.swrobotics.robot.RobotContainer; import com.swrobotics.robot.config.IOAllocation; @@ -80,6 +81,10 @@ private void showShooterStatus() { } } + private void showCoastDriving() { + applySolid(Color.kMagenta); + } + private void showAutoDriving() { // Rainbow applyStripes(5f, @@ -104,13 +109,15 @@ private void showIdle() { @Override public void periodic() { // Special indicator to swap battery - boolean batteryLow = RobotController.getBatteryVoltage() < 10; + boolean batteryLow = RobotController.getBatteryVoltage() < 11; boolean resetShooterBlink = true; if (batteryLowDebounce.calculate(batteryLow)) { showLowBattery(); } else if (DriverStation.isDisabled()) { prideSequencer.apply(this); + } else if (robot.drive.getCurrentNeutralMode() == NeutralModeValue.Coast) { + showCoastDriving(); } else if (robot.shooter.isPreparing()) { resetShooterBlink = false; showShooterStatus(); diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java index 0e984e2..e600805 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.SteerRequestType; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.pathfinding.Pathfinding; @@ -36,6 +37,7 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.DriverStation.MatchType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.swrobotics.lib.net.NTEntry; @@ -87,6 +89,7 @@ public static record TurnRequest(int priority, Rotation2d turn) { private DriveRequest currentDriveRequest; private TurnRequest currentTurnRequest; private int lastSelectedPriority; + private NeutralModeValue currentNeutralMode; public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) { this.fieldInfo = fieldInfo; @@ -116,6 +119,7 @@ public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) { prevPositions = null; currentDriveRequest = NULL_DRIVE; currentTurnRequest = NULL_TURN; + currentNeutralMode = NeutralModeValue.Brake; // Configure pathing AutoBuilder.configureHolonomic( @@ -240,6 +244,15 @@ public void periodic() { } } + // Coast if we are really close to the end of the match + // (lets us slide into the stage at the end to clutch up the 1 pt) + double time = DriverStation.getMatchTime(); + if (DriverStation.isTeleopEnabled() && time < 3) { + setNeutralMode(NeutralModeValue.Coast); + } else { + setNeutralMode(NeutralModeValue.Brake); + } + // Update estimator // Do refresh here, so we get the most up-to-date data SwerveModulePosition[] positions = getCurrentModulePositions(true); @@ -317,7 +330,21 @@ public TalonFX getTurnMotor(int module) { return modules[module].getSteerMotor(); } + public void setNeutralMode(NeutralModeValue neutralMode) { + if (neutralMode == currentNeutralMode) + return; + + currentNeutralMode = neutralMode; + for (SwerveModule module : modules) { + module.configNeutralMode(neutralMode); + } + } + public void setEstimatorIgnoreVision(boolean ignoreVision) { estimator.setIgnoreVision(ignoreVision); } + + public NeutralModeValue getCurrentNeutralMode() { + return currentNeutralMode; + } }