Power conservation: current limits, compressor inhibit, flywheel gating

src/main/java/frc/game/GameState.java

@@ -116,6 +116,17 @@ public static Optional<Alliance> 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())) {

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());
   }
 }

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;

src/main/java/frc/robot/Robot.java

@@ -141,8 +141,11 @@ public class Robot extends LoggedRobot {
   private LoggedCompressor compressor;
   private PneumaticsSimulator pneumaticsSimulator;
 
-  // Battery simulation constants
-  private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc
+  // 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
   private final LinearFilter vBusFilter = LinearFilter.singlePoleIIR(0.04, Robot.defaultPeriodSecs);
 
   public Robot() {
@@ -329,14 +332,24 @@ 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")));
+    // 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. */
@@ -418,6 +431,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 +449,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();
   }
@@ -488,7 +504,7 @@ public void simulationPeriodic() {
                     feeder.getSimCurrentDrawAmps(),
                     intake.getSimCurrentDrawAmps(),
                     pneumaticsSimulator.getCompressorCurrentAmps(),
-                    ELECTRONICS_OVERHEAD_AMPS))));
+                    Constants.PowerConstants.kElectronicsOverheadAmps))));
   }
 
   private void configureControlPanelBindings() {
@@ -508,6 +524,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 +580,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());
 
@@ -750,7 +756,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 +797,48 @@ 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 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 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.deadlineWith(
+        Commands.run(compressor::disable, compressor).withName("Shot inhibit"));
+  }
+
   private Command createDesaturateAndShootCommand(DriverController driver) {
     return Commands.parallel(
         DriveCommands.joystickDrive(
@@ -809,7 +857,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() {
@@ -840,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();
   }
 

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)

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

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

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;
 

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;

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;