Odometry when tilted -> master

build.gradle

@@ -107,14 +107,9 @@ dependencies {
 
 
 repositories {
-	maven {
-		url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
-		credentials {
-			username = "Mechanical-Advantage-Bot"
-			password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
-		}
-	}
-	mavenLocal()
+    maven {
+        url = uri("https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release")
+    }
 }
 
 test {

src/main/java/frc/robot/odometry/OdometryPlus.java

@@ -0,0 +1,62 @@
+package frc.robot.odometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.Odometry;
+import java.util.function.Supplier;
+
+public class OdometryPlus<T> extends Odometry<T> {
+
+	private final Supplier<Rotation3d> robotOrientationSupplier;
+
+	/**
+	 * Constructs a 2d odometry object that takes into consideration the robot's 3d orientation.
+	 *
+	 * @param kinematics        The kinematics of the chassis.
+	 * @param gyroAngle         The given robot's yaw.
+	 * @param wheelPositions    The current wheel positions.
+	 * @param initialPoseMeters The starting position of the robot on the field.
+	 */
+	public OdometryPlus(Kinematics<?, T> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPoseMeters) {
+		super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
+		this.robotOrientationSupplier = () -> new Rotation3d(0, 0, this.getHeading().getRadians());
+	}
+
+	public OdometryPlus(
+		Kinematics<?, T> kinematics,
+		Rotation2d gyroAngle,
+		T wheelPositions,
+		Pose2d initialPoseMeters,
+		Supplier<Rotation3d> robotOrientationSupplier
+	) {
+		super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
+		this.robotOrientationSupplier = robotOrientationSupplier;
+	}
+
+	@Override
+	public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+		Pose2d lastPose = getPoseMeters();
+		Pose2d unCompensatedPose = super.update(gyroAngle, wheelPositions);
+
+		if (robotOrientationSupplier == null)
+			return unCompensatedPose;
+		Rotation3d robotOrientation = robotOrientationSupplier.get();
+		double scale = Math.cos(robotOrientation.rotateBy(new Rotation3d(0, 0, -robotOrientation.getZ())).getY());
+		Transform2d unCompensatedDistance = new Transform2d(lastPose, unCompensatedPose);
+
+		Pose2d compensatedByPitch = lastPose
+			.plus(new Transform2d(unCompensatedDistance.getTranslation().times(scale), unCompensatedDistance.getRotation()));
+
+		resetTranslation(compensatedByPitch.getTranslation());
+
+		return compensatedByPitch;
+	}
+
+	public Rotation2d getHeading() {
+		return this.getPoseMeters().getRotation();
+	}
+
+}