diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index d1eaff4c..627092b7 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -24,7 +24,7 @@ import java.util.concurrent.locks.ReentrantLock; import java.util.function.Supplier; import java.util.stream.IntStream; -import lombok.Getter; +import lombok.Setter; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.Constants; import org.littletonrobotics.frc2024.RobotState; @@ -51,7 +51,7 @@ public class Drive extends SubsystemBase { private static final LoggedTunableNumber coastMetersPerSecThreshold = new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", 0.05); - public enum DriveMode { + public static enum DriveMode { /** Driving with input from driver joysticks. (Default) */ TELEOP, @@ -68,6 +68,12 @@ public enum DriveMode { WHEEL_RADIUS_CHARACTERIZATION } + public static enum CoastRequest { + AUTOMATIC, + ALWAYS_BRAKE, + ALWAYS_COAST + } + @AutoLog public static class OdometryTimestampInputs { public double[] timestamps = new double[] {}; @@ -93,10 +99,15 @@ public static class OdometryTimestampInputs { private boolean modulesOrienting = false; private final Timer lastMovementTimer = new Timer(); - @Getter @AutoLogOutput(key = "Drive/BrakeModeEnabled") private boolean brakeModeEnabled = true; + @Setter + @AutoLogOutput(key = "Drive/CoastRequest") + private CoastRequest coastRequest = CoastRequest.AUTOMATIC; + + private boolean lastEnabled = false; + private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); private SwerveSetpoint currentSetpoint = @@ -122,6 +133,7 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) modules[2] = new Module(bl, 2); modules[3] = new Module(br, 3); lastMovementTimer.start(); + setBrakeMode(true); setpointGenerator = SwerveSetpointGenerator.builder() @@ -220,6 +232,26 @@ public void periodic() { .anyMatch(module -> module.getVelocityMetersPerSec() > coastMetersPerSecThreshold.get())) { lastMovementTimer.reset(); } + if (DriverStation.isEnabled() && !lastEnabled) { + coastRequest = CoastRequest.AUTOMATIC; + } + + lastEnabled = DriverStation.isEnabled(); + switch (coastRequest) { + case AUTOMATIC -> { + if (DriverStation.isEnabled()) { + setBrakeMode(true); + } else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) { + setBrakeMode(false); + } + } + case ALWAYS_BRAKE -> { + setBrakeMode(true); + } + case ALWAYS_COAST -> { + setBrakeMode(false); + } + } // Run drive based on current mode ChassisSpeeds teleopSpeeds = teleopDriveController.update(); @@ -401,6 +433,14 @@ public double[] getWheelRadiusCharacterizationPosition() { return Arrays.stream(modules).mapToDouble(Module::getPositionRads).toArray(); } + /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */ + private void setBrakeMode(boolean enabled) { + if (brakeModeEnabled != enabled) { + Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); + } + brakeModeEnabled = enabled; + } + /** * Returns command that orients all modules to {@code orientation}, ending when the modules have * rotated. diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java index c30a27fc..19efe2ae 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java @@ -94,6 +94,12 @@ public void runCharacterization(double turnSetpointRads, double input) { io.runCharacterization(input); } + /** Sets brake mode to {@code enabled}. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + /** Stops motors. */ public void stop() { io.stop(); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java index 96d9467d..3f283478 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java @@ -22,6 +22,8 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; import java.util.Queue; +import java.util.concurrent.Executor; +import java.util.concurrent.Executors; import java.util.function.Supplier; public class ModuleIOKrakenFOC implements ModuleIO { @@ -52,6 +54,7 @@ public class ModuleIOKrakenFOC implements ModuleIO { // Controller Configs private final TalonFXConfiguration driveTalonConfig = new TalonFXConfiguration(); private final TalonFXConfiguration turnTalonConfig = new TalonFXConfiguration(); + private static final Executor brakeModeExecutor = Executors.newFixedThreadPool(8); // Control private final VoltageOut voltageControl = new VoltageOut(0).withUpdateFreqHz(0); @@ -227,16 +230,26 @@ public void setTurnPID(double kP, double kI, double kD) { @Override public void setDriveBrakeMode(boolean enable) { - driveTalonConfig.MotorOutput.NeutralMode = - enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(driveTalonConfig); + brakeModeExecutor.execute( + () -> { + synchronized (driveTalonConfig) { + driveTalonConfig.MotorOutput.NeutralMode = + enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(driveTalonConfig, 0.25); + } + }); } @Override public void setTurnBrakeMode(boolean enable) { - turnTalonConfig.MotorOutput.NeutralMode = - enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(turnTalonConfig); + brakeModeExecutor.execute( + () -> { + synchronized (turnTalonConfig) { + turnTalonConfig.MotorOutput.NeutralMode = + enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(turnTalonConfig, 0.25); + } + }); } @Override diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java index 66e307af..1a365857 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -11,11 +11,13 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import org.littletonrobotics.frc2024.Constants; +import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.ModuleConfig; public class ModuleIOSim implements ModuleIO { private final DCMotorSim driveSim = @@ -32,6 +34,9 @@ public class ModuleIOSim implements ModuleIO { private double turnAppliedVolts = 0.0; private final Rotation2d turnAbsoluteInitPosition; + private boolean driveCoast = false; + private SlewRateLimiter driveVoltsLimiter = new SlewRateLimiter(4.0); + public ModuleIOSim(ModuleConfig config) { turnAbsoluteInitPosition = config.absoluteEncoderOffset(); turnFeedback.enableContinuousInput(-Math.PI, Math.PI); @@ -43,6 +48,12 @@ public void updateInputs(ModuleIOInputs inputs) { stop(); } + if (driveCoast && DriverStation.isDisabled()) { + runDriveVolts(driveVoltsLimiter.calculate(driveAppliedVolts)); + } else { + driveVoltsLimiter.reset(driveAppliedVolts); + } + driveSim.update(Constants.loopPeriodSecs); turnSim.update(Constants.loopPeriodSecs); @@ -101,6 +112,11 @@ public void setTurnPID(double kP, double kI, double kD) { turnFeedback.setPID(kP, kI, kD); } + @Override + public void setDriveBrakeMode(boolean enable) { + driveCoast = !enable; + } + @Override public void stop() { runDriveVolts(0.0);