Gyro + Field Orientation Helper

Disclaimer: WIP and untested; set your gyro type and offsets. Custom code built on WPILib APIs.

Code (Java/WPILib)

public class GyroHelper {
  private final AHRS gyro = new AHRS(SPI.Port.kMXP);
  private Rotation2d offset = new Rotation2d();
  private final PIDController holdPid = new PIDController(0.02, 0, 0.001);

  public Rotation2d heading() {
    return gyro.getRotation2d().plus(offset);
  }

  public void zero(Rotation2d desiredZero) {
    offset = desiredZero.minus(gyro.getRotation2d());
  }

  public double headingHoldOutput(Rotation2d targetHeading) {
    return MathUtil.clamp(holdPid.calculate(heading().getDegrees(), targetHeading.getDegrees()), -0.5, 0.5);
  }
}