Add odometry commands

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -10,12 +10,14 @@
 import com.stuypulse.robot.commands.launcher.LaunchPrepare;
 import com.stuypulse.robot.commands.launcher.LauncherIntakeNote;
 import com.stuypulse.robot.commands.launcher.LauncherStop;
+import com.stuypulse.robot.commands.odometry.OdometryRealign;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.subsystems.drivetrain.Drivetrain;
 import com.stuypulse.robot.subsystems.launcher.Launcher;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -54,6 +56,11 @@ private void configureDefaultCommands() {
     /***************/
 
     private void configureButtonBindings() {
+        configureDriverBindings();
+        configureOperatorBindings();
+    }
+
+    private void configureDriverBindings() {
         operator.getLeftBumper()
             .onTrue(new LauncherIntakeNote())
             .onFalse(new LauncherStop());
@@ -64,6 +71,14 @@ private void configureButtonBindings() {
             .onFalse(new LauncherStop());
     }
 
+    private void configureOperatorBindings() {
+        // odometry
+        driver.getDPadUp().onTrue(new OdometryRealign(Rotation2d.fromDegrees(180)));
+        driver.getDPadLeft().onTrue(new OdometryRealign(Rotation2d.fromDegrees(-90)));
+        driver.getDPadDown().onTrue(new OdometryRealign(Rotation2d.fromDegrees(0)));
+        driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90)));
+    }
+
     /**************/
     /*** AUTONS ***/
     /**************/

src/main/java/com/stuypulse/robot/commands/odometry/OdometryRealign.java

@@ -0,0 +1,36 @@
+package com.stuypulse.robot.commands.odometry;
+
+import com.stuypulse.robot.subsystems.odometry.AbstractOdometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+import java.util.function.Supplier;
+
+public class OdometryRealign extends InstantCommand {
+    private final AbstractOdometry odometry;
+    private final Supplier<Rotation2d> references;
+
+    public OdometryRealign(Supplier<Rotation2d> references) {
+        odometry = AbstractOdometry.getInstance();
+        this.references = references;
+
+        // addRequirements(odometry);
+    }
+
+    public OdometryRealign(Rotation2d reference) {
+        this(() -> reference);
+    }
+
+    public OdometryRealign() {
+        this(Rotation2d.fromDegrees(0));
+    }
+
+    @Override
+    public void initialize() {
+        Pose2d pose = odometry.getPose();
+        odometry.resetOdometery(new Pose2d(pose.getTranslation(), references.get()));
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/odometry/OdometryReset.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.odometry;
+
+import com.stuypulse.robot.subsystems.odometry.AbstractOdometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+import java.util.function.Supplier;
+
+public class OdometryReset extends InstantCommand {
+    private final AbstractOdometry odometry;
+    private final Supplier<Pose2d> references;
+
+    public OdometryReset(Supplier<Pose2d> references) {
+        odometry = AbstractOdometry.getInstance();
+        this.references = references;
+
+        // addRequirements(null);
+    }
+
+    public OdometryReset(Pose2d reference) {
+        this(() -> reference);
+    }
+
+    @Override
+    public void initialize() {
+        odometry.resetOdometery(references.get());
+    }
+
+}