Closed-Loop Control: PID + Feedforward for a Consistent Shot

In Project 4 you ran the launcher open-loop at 85%. The problem: after each ball, the wheel slows down and the next shot is weaker until it recovers. Closed-loop control fixes this by measuring the wheel's speed and correcting toward a target. For REBUILT's Hub scoring, shot consistency is what turns a good launcher into a ranking-point machine.

The two halves. WPILib's recommended approach combines feedforward and feedback:

• Feedforward predicts the voltage needed to hold a speed, using a physics model. For a flywheel: voltage = kS + kV*velocity + kA*acceleration. The SimpleMotorFeedforward class implements exactly this.
• Feedback (PID) corrects the small remaining error. PIDController computes a correction from the difference between the target and the measured velocity.

You add their outputs together — feedforward does the heavy lifting, PID cleans up.

Getting the gains with SysId. Do not guess kS/kV/kA. WPILib's SysId tool does this rigorously: you add a characterization routine that drives the mechanism through quasi-static and dynamic tests while logging voltage and velocity. After the run, you load the log into the SysId app, which fits a model and outputs kS, kV, kA, plus suggested PID gains. (For a drivetrain, SysId needs clear travel space; a flywheel needs none.)

private final SimpleMotorFeedforward m_ff =
    new SimpleMotorFeedforward(kS, kV, kA);   // from SysId
private final PIDController m_pid =
    new PIDController(kP, 0, 0);               // from SysId

public void runFlywheel(double targetRPS) {
  double measured = m_encoder.getVelocity();  // rotations/sec
  double ff  = m_ff.calculate(targetRPS);
  // PIDController.calculate(measurement, setpoint): measured first
  double fb  = m_pid.calculate(measured, targetRPS);
  m_motor.setVoltage(ff + fb);
}

Note the argument order: PIDController.calculate(measurement, setpoint) takes the measured value first and the setpoint second — getting these backwards is a common bug.

Tuning intuition. With good feedforward, the wheel reaches and holds target with very little PID effort. Raise kP until the wheel recovers quickly after a shot without oscillating. If it oscillates, lower kP. The WPILib flywheel-tuning guide walks through this exact loop. Velocity control like this is forgiving — flywheels are nearly the textbook case for feedforward.

The payoff: a characterized launcher returns to target RPM quickly between shots, so every Fuel leaves at the same speed and lands in the Hub. That repeatability is what lets a team chain enough scores to reach the Energized (100 Fuel) and Supercharged (360 Fuel) ranking-point thresholds reliably instead of hoping.