From 7527a67786bfb35b60d6bf1fba16d1a2e9b090f5 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 26 Mar 2026 19:51:36 -0400 Subject: [PATCH 1/4] Reduce subsystem current limits to improve brownout margin MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Addresses VACHE analysis findings (doc/VACHE_POWER_ANALYSIS.md): - Spindexer + Kicker: add secondaryCurrentLimit(80A) as a hard cap. The existing smartCurrentLimit(60A) is velocity-dependent and does not prevent transient stalls; four 149A stall events across Q54/Q13 were the direct cause of the worst voltage dips at VACHE. - Intake rollers: stator limit reduced 100A → 60A. Observed VACHE peaks were 56A; this matches the operating envelope with no performance impact. - Flywheel: stator limit reduced 100A → 60A per motor. Observed spin-up peaks touched 60A; capping at the observed ceiling avoids excess headroom contributing to battery sag. Drive motor limits unchanged per analysis recommendation. Co-Authored-By: Claude Sonnet 4.6 --- .../java/frc/robot/subsystems/feeder/FeederConstants.java | 4 ++++ .../java/frc/robot/subsystems/feeder/KickerIOSpark.java | 1 + .../java/frc/robot/subsystems/feeder/SpindexerIOSpark.java | 1 + .../java/frc/robot/subsystems/intake/IntakeConstants.java | 3 +++ .../java/frc/robot/subsystems/intake/RollerIOTalonFX.java | 6 +++--- .../frc/robot/subsystems/launcher/FlywheelIOTalonFX.java | 6 +++--- .../frc/robot/subsystems/launcher/LauncherConstants.java | 3 +++ 7 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index dcb6062f..748bb7bf 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -17,6 +17,8 @@ public static final class SpindexerConstants { // Motor public static final double motorReduction = 1.0; + // Hard secondary current limit to prevent 149A stall events (VACHE analysis) + public static final int kSecondaryCurrentLimit = 80; public static final LinearVelocity maxTangentialVelocity = MetersPerSecond.of( NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) @@ -39,6 +41,8 @@ public static final class KickerConstants { // Motor public static final double motorReduction = 1.0; + // Hard secondary current limit to prevent 149A stall events (VACHE analysis) + public static final int kSecondaryCurrentLimit = 80; public static final LinearVelocity maxTangentialVelocity = MetersPerSecond.of( NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java index a1b7e430..5a902b7c 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -41,6 +41,7 @@ public KickerIOSpark() { .inverted(false) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .secondaryCurrentLimit(kSecondaryCurrentLimit) .voltageCompensation(RobotConstants.kNominalVoltage); config diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java index 5b6403f8..d67a3481 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -41,6 +41,7 @@ public SpindexerIOSpark() { .inverted(false) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .secondaryCurrentLimit(kSecondaryCurrentLimit) .voltageCompensation(RobotConstants.kNominalVoltage); config diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f8e53da6..afb4a536 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -27,6 +27,9 @@ public static class RollerConstants { public static final Slot1Configs velocityTorqueCurrentGains = new Slot1Configs().withKP(5).withKI(0.0).withKD(0.0).withKS(2.5); + // Stator current limit; reduced from default 100A — observed VACHE peaks were 56A + public static final int kStatorCurrentLimit = 60; + public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0; diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index 7619a050..0bee629d 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -42,9 +42,9 @@ public class RollerIOTalonFX implements RollerIO { public RollerIOTalonFX(RollerConfig rollerConfig) { motor = new TalonFX(rollerConfig.port, rollerConfig.bus); config = new TalonFXConfiguration(); - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.kDefaultStatorCurrentLimit; - config.TorqueCurrent.PeakReverseTorqueCurrent = -KrakenX60Constants.kDefaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = kStatorCurrentLimit; + config.TorqueCurrent.PeakReverseTorqueCurrent = -kStatorCurrentLimit; + config.CurrentLimits.StatorCurrentLimit = kStatorCurrentLimit; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.kDefaultSupplyCurrentLimit; config.CurrentLimits.SupplyCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 9ea59578..dd58b138 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -56,9 +56,9 @@ public FlywheelIOTalonFX() { .withNeutralMode(NeutralModeValue.Brake); config.Slot0 = velocityVoltageGains; config.Slot1 = velocityTorqueCurrentGains; - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.kDefaultStatorCurrentLimit; - config.TorqueCurrent.PeakReverseTorqueCurrent = -KrakenX60Constants.kDefaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = kStatorCurrentLimit; + config.TorqueCurrent.PeakReverseTorqueCurrent = -kStatorCurrentLimit; + config.CurrentLimits.StatorCurrentLimit = kStatorCurrentLimit; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.kDefaultSupplyCurrentLimit; config.CurrentLimits.SupplyCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 7fb9e4d2..beafd831 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -111,6 +111,9 @@ public static final class FlywheelScaling { public static final Distance wheelRadius = Inches.of(1.5); + // Stator current limit; reduced from default 100A — spin-up peaks observed at 40-60A/motor + public static final int kStatorCurrentLimit = 60; + // Velocity Controller public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0; From 7893ca8261b67bdf2b8d997b2ab9f4c7213205f0 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 26 Mar 2026 19:59:06 -0400 Subject: [PATCH 2/4] Inhibit compressor during shot cycles MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Wraps the spin-forward command (all three trigger sites — operator A, driver A, and right bumper desaturate-and-shoot) with compressor::disable before start and compressor::enableDigital on end. The compressor was present and adding 6-20A at every major voltage dip in the VACHE analysis; in Q54 it ramped to 19.6A at the exact worst moment (6.67V). The 70in³ storage tank provides 30+ seconds of reserve, so brief inhibits during shot cycles have no operational impact. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Robot.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a1801a16..389fe75f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -750,7 +750,7 @@ public void bindXboxOperator(int port, DriverController driver) { // xboxOperator.y().and(() -> hopper.isDeployed()).whileTrue(intake.getReverseCommand()); // Feeder - xboxOperator.a().whileTrue(feeder.getSpinForwardCommand()); + xboxOperator.a().whileTrue(shootWithCompressorInhibit()); xboxOperator.x().whileTrue(feeder.getReverseCommand()); @@ -791,6 +791,16 @@ public static long getUSBStorageFreeSpace() { return new java.io.File("/U").getFreeSpace(); } + /** + * Returns the spin-forward command with the compressor disabled for its duration. Prevents the + * compressor from adding 6-20A during shot cycles (VACHE power analysis recommendation). + */ + private Command shootWithCompressorInhibit() { + var cmd = feeder.getSpinForwardCommand(); + if (compressor == null) return cmd; + return cmd.beforeStarting(compressor::disable).finallyDo(compressor::enableDigital); + } + private Command createDesaturateAndShootCommand(DriverController driver) { return Commands.parallel( DriveCommands.joystickDrive( @@ -809,7 +819,7 @@ private Command createDesaturateAndShootCommand(DriverController driver) { allianceSelector::fieldRotated) .withName("Desaturate turret"), Commands.sequence( - Commands.waitUntil(launcher::isTurretDesaturated), feeder.getSpinForwardCommand())); + Commands.waitUntil(launcher::isTurretDesaturated), shootWithCompressorInhibit())); } private static void logHIDs() { From f0c03d25a4e4281df63696b8d1bcbeb403c5334e Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 26 Mar 2026 20:47:25 -0400 Subject: [PATCH 3/4] Flywheel power conservation: dial policy, hub-active zone gating, at-speed shot gate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Launcher.aim() takes policyEnabled; flywheel off when policy false and force-enable not active - Turret falls back to nominal aim direction when flywheel is off/spinning up (<0.5 m/s ballistic) - flywheelForceEnabled / isFlywheelAtSpeed() gate the feeder: spin up before advancing - hoodInitialized flag prevents isFlywheelAtSpeed() returning true during hood initialization - kAtSpeedToleranceFraction = 0.05 (validated against VACHE logs: steady-state error 1-3%) - Separate createAutoLauncherDefault() (always policyEnabled=true) and createTeleopLauncherDefault() (flywheelPolicy && isMyHubActive && isInMyAllianceZone); set at mode transitions, not constructor - flywheelPolicy BooleanSupplier: defaults to always-true; Zorro driver overrides with dial gate (FMS bypass: dial ignored when FMS attached) - GameState: add getMyHub(), isInMyAllianceZone() - Remove dead Zorro launcher trigger (was scheduling the default command as whileTrue) - shootWithCompressorInhibit: sequence force-enable → waitUntil(atSpeed) → feeder Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/game/GameState.java | 15 ++++ src/main/java/frc/robot/Robot.java | 74 ++++++++++++------- .../robot/subsystems/launcher/Launcher.java | 60 +++++++++++++-- .../launcher/LauncherConstants.java | 4 + 4 files changed, 119 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/game/GameState.java b/src/main/java/frc/game/GameState.java index 6562aa43..34d1921a 100644 --- a/src/main/java/frc/game/GameState.java +++ b/src/main/java/frc/game/GameState.java @@ -116,6 +116,17 @@ public static Optional getAlliance() { return DriverStation.getAlliance(); } + /** + * Returns true if the given pose is inside our alliance zone (within allianceZone_x_len of our + * end wall). Returns false when alliance is unknown. + */ + public static boolean isInMyAllianceZone(Pose2d pose) { + Alliance alliance = getMyAlliance(); + if (alliance == Alliance.Blue) return Field.Region.BlueZone.contains(pose); + if (alliance == Alliance.Red) return Field.Region.RedZone.contains(pose); + return false; + } + public static void logValues() { getMyAlliance(); getAutoWinner(); @@ -131,6 +142,10 @@ public static void logValues() { Logger.recordOutput("GameState/IsMyHubActive", isMyHubActive()); } + public static Pose3d getMyHub() { + return Robot.getAlliance() == Alliance.Red ? Field.redHubCenter : Field.blueHubCenter; + } + public static Pose3d getTarget(Pose2d robotPose) { if (Robot.getAlliance() == Alliance.Red) { if (robotPose.getMeasureX().gt(Field.redHubCenter.getMeasureX())) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 389fe75f..80040473 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -141,6 +141,10 @@ public class Robot extends LoggedRobot { private LoggedCompressor compressor; private PneumaticsSimulator pneumaticsSimulator; + // Flywheel policy: true = flywheel permitted by driver input. Overridden by bindZorroDriver; + // defaults to always-enabled for Xbox/keyboard drivers that lack a dial. + private java.util.function.BooleanSupplier flywheelPolicy = () -> true; + // Battery simulation constants private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc private final LinearFilter vBusFilter = LinearFilter.singlePoleIIR(0.04, Robot.defaultPeriodSecs); @@ -329,14 +333,6 @@ public Robot() { feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop")); intake.setDefaultCommand(intake.getDefaultCommand()); - launcher.setDefaultCommand( - launcher - .initializeHoodCommand() - .andThen( - new RunCommand( - () -> launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()), - launcher) - .withName("Aim at hub"))); } /** This function is called periodically during all modes. */ @@ -418,6 +414,7 @@ public void disabledPeriodic() { public void autonomousInit() { if (hopper != null) hopper.getRetractCommand().schedule(); drive.setDefaultCommand(Commands.runOnce(drive::stop, drive).withName("Stop")); + launcher.setDefaultCommand(createAutoLauncherDefault()); autoSelector.scheduleAuto(); leds.clear(); } @@ -435,6 +432,8 @@ public void teleopInit() { if (hopper != null && !hopper.isDeployed() && !hopper.isStowed()) hopper.getRetractCommand().schedule(); autoSelector.cancelAuto(); + flywheelPolicy = () -> true; // reset before binding; overridden below if Zorro is driver + launcher.setDefaultCommand(createTeleopLauncherDefault()); ControllerSelector.getInstance().scan(true); leds.clear(); } @@ -508,6 +507,10 @@ private void configureControlPanelBindings() { public DriverController bindZorroDriver(int port) { var zorroDriver = new CommandZorroController(port); + flywheelPolicy = + () -> + DriverStation.isFMSAttached() + || zorroDriver.axisGreaterThan(Axis.kLeftDial.value, 0.5).getAsBoolean(); var controller = new DriverController() { @@ -560,20 +563,6 @@ public boolean getFieldRelativeInput() { // Desaturate turret and advance feeder zorroDriver.AIn().whileTrue(createDesaturateAndShootCommand(controller)); - // Launcher - Trigger launcherEnabled = zorroDriver.axisGreaterThan(Axis.kLeftDial.value, 0.5).debounce(0.1); - launcherEnabled - .or(() -> DriverStation.isFMSAttached()) - .whileTrue( - launcher - .initializeHoodCommand() - .andThen( - new RunCommand( - () -> - launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()), - launcher) - .withName("Aim at hub"))); - // Intake zorroDriver.HIn().whileTrue(intake.getDeployCommand()); @@ -791,14 +780,45 @@ public static long getUSBStorageFreeSpace() { return new java.io.File("/U").getFreeSpace(); } + private Command createAutoLauncherDefault() { + return launcher + .initializeHoodCommand() + .andThen( + new RunCommand( + () -> launcher.aim(GameState.getMyHub().getTranslation(), true), launcher) + .withName("Aim at hub")); + } + + private Command createTeleopLauncherDefault() { + return launcher + .initializeHoodCommand() + .andThen( + new RunCommand( + () -> + launcher.aim( + GameState.getTarget(drive.getPose()).getTranslation(), + flywheelPolicy.getAsBoolean() + && GameState.isMyHubActive() + && GameState.isInMyAllianceZone(drive.getPose())), + launcher) + .withName("Aim at hub")); + } + /** - * Returns the spin-forward command with the compressor disabled for its duration. Prevents the - * compressor from adding 6-20A during shot cycles (VACHE power analysis recommendation). + * Returns the shoot command with compressor inhibited and flywheel force-enabled for its + * duration. Force-enables the flywheel (overriding hub/zone policy), waits for it to reach speed, + * then advances the feeder. Compressor is disabled for the full shot cycle to prevent 6-20A + * interference (VACHE power analysis recommendation). */ private Command shootWithCompressorInhibit() { - var cmd = feeder.getSpinForwardCommand(); - if (compressor == null) return cmd; - return cmd.beforeStarting(compressor::disable).finallyDo(compressor::enableDigital); + var innerCmd = + Commands.sequence( + Commands.runOnce(() -> launcher.setFlywheelForceEnabled(true)), + Commands.waitUntil(launcher::isFlywheelAtSpeed), + feeder.getSpinForwardCommand()) + .finallyDo(() -> launcher.setFlywheelForceEnabled(false)); + if (compressor == null) return innerCmd; + return innerCmd.beforeStarting(compressor::disable).finallyDo(compressor::enableDigital); } private Command createDesaturateAndShootCommand(DriverController driver) { diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index cc4e621c..c8b9764e 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants; import frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.FlywheelScaling; import java.util.ArrayList; import java.util.function.Supplier; @@ -68,6 +69,14 @@ public class Launcher extends SubsystemBase { private double ballisticSimTimer = 0.0; private double ballisticLogTimer = 0.0; + // Flywheel enable state + // policyEnabled is set by aim() callers based on hub-active / alliance-zone checks. + // forceEnabled is set by shot commands to override the policy and spin up before shooting. + // hoodInitialized gates isFlywheelAtSpeed() to prevent shooting during hood initialization. + private boolean flywheelForceEnabled = false; + private boolean hoodInitialized = false; + private double flywheelSetpointMetersPerSec = 0.0; // last computed setpoint, even while disabled + // Turret desaturation private final PIDController headingController = new PIDController(1.5, 0.0, 0.1); @@ -175,7 +184,11 @@ public double getSimCurrentDrawAmps() { return flywheelInputs.currentAmps * 2 + turretInputs.currentAmps + hoodInputs.currentAmps; } - public void aim(Translation3d target) { + /** + * @param policyEnabled whether hub-active / alliance-zone conditions permit the flywheel to run. + * Ignored when {@link #setFlywheelForceEnabled} is active (e.g. during a shot command). + */ + public void aim(Translation3d target, boolean policyEnabled) { long aimStart = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get vector from static target to turret @@ -187,10 +200,17 @@ public void aim(Translation3d target) { Math.hypot(vectorTurretBaseToTarget.getX(), vectorTurretBaseToTarget.getY()); Rotation2d dynamicImpactAngle = getImpactAngle(horizontalDistance); - // Set flywheel speed assuming a motionless robot + // Compute nominal flywheel setpoint (always needed for turret aim direction) var v0_nominal = getV0Nominal(vectorTurretBaseToTarget, dynamicImpactAngle, nominalKey); var flywheelSetpoint = MetersPerSecond.of(flywheelSetpointfromBallistics(v0_nominal.getNorm())); - flywheelIO.setVelocity(flywheelSetpoint); + flywheelSetpointMetersPerSec = flywheelSetpoint.in(MetersPerSecond); + + // Spin flywheel only when policy or force-enable permits + if (policyEnabled || flywheelForceEnabled) { + flywheelIO.setVelocity(flywheelSetpoint); + } else { + flywheelIO.setOpenLoop(Volts.of(0.0)); + } long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get translation velocities (m/s) of the turret caused by motion of the chassis @@ -205,12 +225,19 @@ public void aim(Translation3d target) { double initialSpeedMetersPerSec = ballisticsFromFlywheelSetpoint(flywheelInputs.velocityMetersPerSec); - // Replan shot using actual initial shot speed speed - var v0_total = getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, replannedKey); + // Replan shot using actual flywheel speed; fall back to nominal direction when the flywheel is + // off or still spinning up so the turret continues tracking the target rather than returning + // early due to a near-zero velocity vector. + Translation3d v0_forAim; + if (initialSpeedMetersPerSec < 0.5) { + v0_forAim = v0_nominal; // flywheel off / spinning up — aim using ideal direction + } else { + v0_forAim = getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, replannedKey); + } long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Point turret to align velocity vectors - var v0_flywheel = v0_total.minus(v_base); + var v0_flywheel = v0_forAim.minus(v_base); // Check if v0_flywheel has non-zero horizontal component double v0_horizontal = Math.hypot(v0_flywheel.getX(), v0_flywheel.getY()); @@ -262,7 +289,7 @@ public void aim(Translation3d target) { fuelNominal.add( new BallisticObject(turretBasePose.getTranslation(), v0_nominal, target.getMeasureZ())); fuelReplanned.add( - new BallisticObject(turretBasePose.getTranslation(), v0_total, target.getMeasureZ())); + new BallisticObject(turretBasePose.getTranslation(), v0_forAim, target.getMeasureZ())); fuelActual.add( new BallisticObject(turretBasePose.getTranslation(), v0_actual, target.getMeasureZ())); } @@ -302,6 +329,23 @@ public Rotation2d getHorizontalAimAngle() { return horizontalAimAngle; } + /** + * Called by shot commands to override hub/zone policy and force the flywheel on so it can spin up + * before the feeder advances. Must be cleared (false) when the shot command ends. + */ + public void setFlywheelForceEnabled(boolean force) { + flywheelForceEnabled = force; + } + + /** True when the flywheel is spinning at its current setpoint within the at-speed tolerance. */ + @AutoLogOutput(key = "Launcher/FlywheelAtSpeed") + public boolean isFlywheelAtSpeed() { + if (!hoodInitialized) return false; + if (flywheelSetpointMetersPerSec < 1e-6) return false; + double error = Math.abs(flywheelInputs.velocityMetersPerSec - flywheelSetpointMetersPerSec); + return error < flywheelSetpointMetersPerSec * FlywheelConstants.kAtSpeedToleranceFraction; + } + public double getTurretDesaturationDelta() { return headingController.calculate(-turretInputs.oversaturationLessMargin, 0); } @@ -548,6 +592,7 @@ public Command initializeHoodCommand() { return new StartEndCommand( // initialize () -> { + hoodInitialized = false; hoodIO.configureSoftLimits(false); hoodIO.setOpenLoop(Volts.of(1.0)); }, @@ -555,6 +600,7 @@ public Command initializeHoodCommand() { () -> { hoodIO.configureSoftLimits(true); hoodIO.resetEncoder(); + hoodInitialized = true; }, // requirements this) diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index beafd831..4d42e718 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -114,6 +114,10 @@ public static final class FlywheelScaling { // Stator current limit; reduced from default 100A — spin-up peaks observed at 40-60A/motor public static final int kStatorCurrentLimit = 60; + // At-speed tolerance: flywheel actual speed must be within this fraction of its setpoint + // before the feeder is allowed to advance during a shot cycle. + public static final double kAtSpeedToleranceFraction = 0.05; + // Velocity Controller public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0; From 3570eda9fc7f4e31e68382a830e42b7f40ae3ab2 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 26 Mar 2026 21:14:59 -0400 Subject: [PATCH 4/4] Inhibit compressor under PDH load via command-based subsystem Makes LoggedCompressor a SubsystemBase so the command scheduler owns compressor state. Default command applies 150A/100A hysteresis against PDH total current; shot commands interrupt it via deadlineWith. Adds PowerConstants (inhibit/resume thresholds, electronics overhead) to Constants.java. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/lib/LoggedCompressor.java | 26 +++++++++++++++----- src/main/java/frc/robot/Constants.java | 17 +++++++++++++ src/main/java/frc/robot/Robot.java | 27 ++++++++++++++++++--- 3 files changed, 60 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/lib/LoggedCompressor.java b/src/main/java/frc/lib/LoggedCompressor.java index 8e47b0f5..5a583836 100644 --- a/src/main/java/frc/lib/LoggedCompressor.java +++ b/src/main/java/frc/lib/LoggedCompressor.java @@ -2,20 +2,34 @@ import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; -public class LoggedCompressor extends Compressor { +public class LoggedCompressor extends SubsystemBase { + private final Compressor compressor; private final String key; public LoggedCompressor(PneumaticsModuleType moduleType, String logKey) { - super(moduleType); + compressor = new Compressor(moduleType); this.key = logKey; } + public void disable() { + compressor.disable(); + } + + public void enableDigital() { + compressor.enableDigital(); + } + + public boolean isEnabled() { + return compressor.isEnabled(); + } + public void log() { - Logger.recordOutput(key + "/Enabled", isEnabled()); - Logger.recordOutput(key + "/PressureSwitch", getPressureSwitchValue()); - Logger.recordOutput(key + "/CurrentAmps", getCurrent()); - Logger.recordOutput(key + "/PressurePSI", getPressure()); + Logger.recordOutput(key + "/Enabled", compressor.isEnabled()); + Logger.recordOutput(key + "/PressureSwitch", compressor.getPressureSwitchValue()); + Logger.recordOutput(key + "/CurrentAmps", compressor.getCurrent()); + Logger.recordOutput(key + "/PressurePSI", compressor.getPressure()); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7eb374fa..2307ce6d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -136,6 +136,23 @@ public static final class CANHD { } } + public static final class PowerConstants { + /** Fixed overhead current (RoboRIO, radio, PDH, coprocessors) used in battery simulation. */ + public static final double kElectronicsOverheadAmps = 4.5; + + /** + * PDH total current above which the compressor is inhibited (prevents inrush during high-demand + * periods). Matches the 150A threshold from the VACHE power analysis. + */ + public static final double kCompressorInhibitAmps = 150.0; + + /** + * PDH total current below which the compressor load inhibit clears. Hysteresis gap prevents + * rapid enable/disable cycling near the inhibit threshold. + */ + public static final double kCompressorResumeAmps = 100.0; + } + public static final class PneumaticChannels { // hopper public static final int hopperForward = 14; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 80040473..a22707d7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -145,8 +145,7 @@ public class Robot extends LoggedRobot { // defaults to always-enabled for Xbox/keyboard drivers that lack a dial. private java.util.function.BooleanSupplier flywheelPolicy = () -> true; - // Battery simulation constants - private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc + // Battery simulation private final LinearFilter vBusFilter = LinearFilter.singlePoleIIR(0.04, Robot.defaultPeriodSecs); public Robot() { @@ -333,6 +332,24 @@ public Robot() { feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop")); intake.setDefaultCommand(intake.getDefaultCommand()); + // Compressor default: enable when PDH load is low, disable when high (hysteresis via + // thresholds). + // Shot commands interrupt this by requiring the compressor subsystem via deadlineWith. + if (compressor != null) { + compressor.setDefaultCommand( + Commands.run( + () -> { + double pdh = powerDistribution.getTotalCurrent(); + if (pdh > Constants.PowerConstants.kCompressorInhibitAmps) { + compressor.disable(); + } else if (pdh < Constants.PowerConstants.kCompressorResumeAmps) { + compressor.enableDigital(); + } + // Between thresholds: maintain current state (hysteresis). + }, + compressor) + .withName("Auto")); + } } /** This function is called periodically during all modes. */ @@ -487,7 +504,7 @@ public void simulationPeriodic() { feeder.getSimCurrentDrawAmps(), intake.getSimCurrentDrawAmps(), pneumaticsSimulator.getCompressorCurrentAmps(), - ELECTRONICS_OVERHEAD_AMPS)))); + Constants.PowerConstants.kElectronicsOverheadAmps)))); } private void configureControlPanelBindings() { @@ -818,7 +835,8 @@ private Command shootWithCompressorInhibit() { feeder.getSpinForwardCommand()) .finallyDo(() -> launcher.setFlywheelForceEnabled(false)); if (compressor == null) return innerCmd; - return innerCmd.beforeStarting(compressor::disable).finallyDo(compressor::enableDigital); + return innerCmd.deadlineWith( + Commands.run(compressor::disable, compressor).withName("Shot inhibit")); } private Command createDesaturateAndShootCommand(DriverController driver) { @@ -870,6 +888,7 @@ private void logScheduler() { logSubsystem("Feeder", feeder); if (hopper != null) logSubsystem("Hopper", hopper); logSubsystem("Intake", intake); + if (compressor != null) logSubsystem("Compressor", compressor); logAlerts(); }