Odometry Helper (No Vision)

Disclaimer: WIP and untested; set your encoders/kinematics properly. Custom code built on WPILib APIs.

Code (Java, tank example)

public class OdometryHelper {
  private final DifferentialDriveOdometry odometry;

  public OdometryHelper(Rotation2d initialHeading) {
    odometry = new DifferentialDriveOdometry(initialHeading);
  }

  public void update(Rotation2d heading, double leftMeters, double rightMeters) {
    odometry.update(heading, leftMeters, rightMeters);
  }

  public Pose2d getPose() {
    return odometry.getPoseMeters();
  }
}