Intake + Shooter Control

Disclaimer: WIP and untested on 2026 hardware; tune gains and thresholds on your robot. Custom code built on WPILib APIs (not official WPILib sample).

Purpose: Basic fuel handling: intake with jam detection, shooter with RPM control, and a “ready to fire” gate.

Shooter velocity (Java/WPILib)

public class ShooterSubsystem extends SubsystemBase {
  private final TalonFX shooter = new TalonFX(30);
  private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.2, 0.11, 0.015);
  private final PIDController pid = new PIDController(0.1, 0.0, 0.0);
  private double targetRpm = 0.0;

  public ShooterSubsystem() {
    shooter.configFactoryDefault();
    shooter.configVoltageCompSaturation(12.0);
    shooter.enableVoltageCompensation(true);
    shooter.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 20);
  }

  public void setTargetRpm(double rpm) { targetRpm = rpm; }
  public double getRpm() { return shooter.getSelectedSensorVelocity() * 600.0 / 2048.0; }

  @Override
  public void periodic() {
    double ffVolts = ff.calculate(targetRpm / 60.0); // convert to rps
    double output = pid.calculate(getRpm(), targetRpm);
    shooter.set(ControlMode.PercentOutput, (ffVolts / 12.0) + output);
  }

  public boolean ready(double toleranceRpm) {
    return Math.abs(getRpm() - targetRpm) < toleranceRpm;
  }
}

Intake with jam clear

public class IntakeSubsystem extends SubsystemBase {
  private final TalonFX intake = new TalonFX(31);
  private final DigitalInput beam = new DigitalInput(1);
  private final Timer jamTimer = new Timer();

  public void in() { intake.set(ControlMode.PercentOutput, 0.5); }
  public void out() { intake.set(ControlMode.PercentOutput, -0.5); }
  public void stop() { intake.set(ControlMode.PercentOutput, 0.0); }

  public boolean jammed(double seconds) {
    return beam.get() && jamTimer.hasElapsed(seconds);
  }

  @Override
  public void periodic() {
    if (beam.get()) jamTimer.start();
    else { jamTimer.stop(); jamTimer.reset(); }
  }
}

Fire command with gate

public class FireCommand extends SequentialCommandGroup {
  public FireCommand(ShooterSubsystem shooter, IndexerSubsystem indexer, Supplier<HubShiftTimer.HubState> hub) {
    addCommands(
      new InstantCommand(() -> shooter.setTargetRpm(4000)),
      new WaitUntilCommand(() -> shooter.ready(100) && hub.get() == HubShiftTimer.HubState.ACTIVE),
      new RunCommand(indexer::feedOut, indexer).withTimeout(1.0),
      new InstantCommand(() -> shooter.setTargetRpm(0))
    );
  }
}

How to test

• Shooter: spin to target, verify RPM holds within tolerance. +- Intake: feed balls; block roller to see jam clear (reverse briefly if added). +- Fire: ensure it won’t fire unless RPM is within tolerance and hub is active.

Pitfalls

• Tune PID/FF for your shooter; numbers above are placeholders.
• Ensure sensor polarity and beam logic match wiring.
• Add a short reverse burst for jam clear if needed.