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());
+ }
+
+}