diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5be8bc2..469286d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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)) diff --git a/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java b/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java new file mode 100644 index 0000000..7b53dc7 --- /dev/null +++ b/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 m_throttle; + SwerveInputStream driveInput; + + public SnapToAngleWithDriver( + SwerveSubsystem drive, + DoubleSupplier x, + DoubleSupplier y, + BooleanSupplier slow, + Optional 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; + } +}