DriveSubsystem Skeleton (Java)

Disclaimer: WIP and untested; tune IDs, inversion, and kinematics for your robot. Custom code built on WPILib APIs (not official WPILib sample).

Code (tank example)

public class DriveSubsystem extends SubsystemBase {
  private final WPI_TalonFX left = new WPI_TalonFX(1);
  private final WPI_TalonFX right = new WPI_TalonFX(2);
  private final DifferentialDrive drive = new DifferentialDrive(left, right);
  private final AHRS gyro = new AHRS(SPI.Port.kMXP);
  private final DifferentialDriveOdometry odometry =
      new DifferentialDriveOdometry(Rotation2d.fromDegrees(0.0));

  public DriveSubsystem() {
    left.setInverted(false);
    right.setInverted(true);
  }

  public void arcade(double fwd, double rot) {
    drive.arcadeDrive(fwd, rot);
  }

  public void resetOdometry(Pose2d pose) {
    gyro.reset();
    odometry.resetPosition(gyro.getRotation2d(), 0.0, 0.0, pose);
  }

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

  @Override
  public void periodic() {
    // TODO: add encoder distances instead of zeros
    odometry.update(gyro.getRotation2d(), 0.0, 0.0);
  }
}