feat: snap to angle

src/main/java/frc/robot/RobotContainer.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.Constants.IOConstants;
 import frc.robot.commands.drive.Drive;
+import frc.robot.commands.drive.SnapToAngleWithDriver;
 import frc.robot.subsystems.ChuteSubsystem;
 import frc.robot.subsystems.CoralHandlerSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
@@ -127,6 +128,26 @@ private void configureButtonBindings() {
           .onTrue(
               Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN)));
 
+      m_rightJoystick
+          .getPOVPressed()
+          .onTrue(new SnapToAngleWithDriver(m_drive, null, null, null, null));
+
+      m_controller
+          .x()
+          .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2)));
+      m_controller
+          .y()
+          .onTrue(
+              Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN)));
+
+      m_controller
+          .rightBumper()
+          .onTrue(Commands.runOnce(m_coral::grab))
+          .onFalse(Commands.runOnce(m_coral::idle));
+      m_controller
+          .leftBumper()
+          .onTrue(Commands.runOnce(m_coral::release))
+          .onFalse(Commands.runOnce(m_coral::idle));
       m_controller
           .rightBumper()
           .onTrue(Commands.runOnce(m_coral::grab))

src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java

@@ -0,0 +1,72 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import java.util.Optional;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import swervelib.SwerveInputStream;
+
+public class SnapToAngleWithDriver extends Command {
+
+  final SwerveSubsystem m_drive;
+  final DoubleSupplier m_x;
+  final DoubleSupplier m_y;
+  final BooleanSupplier m_slow;
+  final Optional<DoubleSupplier> m_throttle;
+  SwerveInputStream driveInput;
+
+  public SnapToAngleWithDriver(
+      SwerveSubsystem drive,
+      DoubleSupplier x,
+      DoubleSupplier y,
+      BooleanSupplier slow,
+      Optional<DoubleSupplier> throttle) {
+    m_drive = drive;
+    m_x = x;
+    m_y = y;
+    m_slow = slow;
+    m_throttle = throttle;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    driveInput =
+        SwerveInputStream.of(
+                m_drive.getSwerveDrive(),
+                () ->
+                    m_x.getAsDouble()
+                        * (m_slow.getAsBoolean()
+                            ? DriveConstants.kDrivingSpeeds[1]
+                            : DriveConstants.kDrivingSpeeds[0])
+                        * m_throttle.orElse(() -> 1.0).getAsDouble(),
+                () ->
+                    m_y.getAsDouble()
+                        * (m_slow.getAsBoolean()
+                            ? DriveConstants.kDrivingSpeeds[1]
+                            : DriveConstants.kDrivingSpeeds[0])
+                        * m_throttle.orElse(() -> 1.0).getAsDouble())
+            .deadband(0.1)
+            .scaleTranslation(0.8)
+            .allianceRelativeControl(true);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drive.driveFieldOriented(driveInput);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}