diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 02ba2c1..5c5e3e6 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/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 ***/ /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/odometry/OdometryRealign.java b/src/main/java/com/stuypulse/robot/commands/odometry/OdometryRealign.java new file mode 100644 index 0000000..c8d39c3 --- /dev/null +++ b/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 references; + + public OdometryRealign(Supplier 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())); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/odometry/OdometryReset.java b/src/main/java/com/stuypulse/robot/commands/odometry/OdometryReset.java new file mode 100644 index 0000000..8cc2bb9 --- /dev/null +++ b/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 references; + + public OdometryReset(Supplier references) { + odometry = AbstractOdometry.getInstance(); + this.references = references; + + // addRequirements(null); + } + + public OdometryReset(Pose2d reference) { + this(() -> reference); + } + + @Override + public void initialize() { + odometry.resetOdometery(references.get()); + } + +}