From f8bcfb139c825d120364534932c85153fcd16265 Mon Sep 17 00:00:00 2001 From: Mason Vogt Date: Thu, 25 Apr 2024 13:10:32 -0500 Subject: [PATCH] [Robot] Added low lob --- .../robot/control/ControlBoard.java | 35 ++++++++++++------- .../speaker/aim/LowLobAimCalculator.java | 11 ++++++ 2 files changed, 34 insertions(+), 12 deletions(-) create mode 100644 Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LowLobAimCalculator.java diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java index 6b836c7..7b3d677 100644 --- a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java +++ b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java @@ -19,6 +19,7 @@ import com.swrobotics.robot.subsystems.speaker.ShooterSubsystem; import com.swrobotics.robot.subsystems.speaker.aim.AmpAimCalculator; import com.swrobotics.robot.subsystems.speaker.aim.LobCalculator; +import com.swrobotics.robot.subsystems.speaker.aim.LowLobAimCalculator; import com.swrobotics.robot.subsystems.speaker.aim.ManualAimCalculator; import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator; import com.swrobotics.robot.subsystems.swerve.SwerveDrive; @@ -40,7 +41,8 @@ public class ControlBoard extends SubsystemBase { * Driver: * Left stick: translation * Right stick X: rotation - * Left bumper: robot relative + * Left bumper: high lob + * Left Trigger: low lob * Right trigger: aim at speaker * Right bumper: spin up flywheel * Dpad Left: Manual subwoofer shot @@ -105,19 +107,28 @@ public ControlBoard(RobotContainer robot) { driver.start.onFalling(() -> robot.drive.setRotation(new Rotation2d())); driver.back.onFalling(() -> robot.drive.setRotation(new Rotation2d())); // Two buttons to reset gyro so the driver can't get confused + // new Trigger(driver.leftBumper::isPressed) + // .onTrue(Commands.runOnce(() -> lobbing++)) + // .whileTrue(new AimTowardsLobCommand(robot.drive, robot.shooter)) + // .whileTrue(Commands.run(() -> robot.shooter.setTempAimCalculator(LobCalculator.INSTANCE))) + // .onFalse(Commands.runOnce(() -> lobbing--)) + // .debounce(0.2, DebounceType.kRising) // Only debounce the shooting + // .onTrue(Commands.runOnce(() -> lobbing++)) + // .onFalse( + // Commands.run(() -> robot.indexer.setFeedToShooter(true)).withTimeout(0.5) + // .andThen(Commands.runOnce(() -> { + // robot.indexer.setFeedToShooter(false); + // lobbing--; + // }))); + new Trigger(driver.leftBumper::isPressed) - .onTrue(Commands.runOnce(() -> lobbing++)) .whileTrue(new AimTowardsLobCommand(robot.drive, robot.shooter)) - .whileTrue(Commands.run(() -> robot.shooter.setTempAimCalculator(LobCalculator.INSTANCE))) - .onFalse(Commands.runOnce(() -> lobbing--)) - .debounce(0.2, DebounceType.kRising) // Only debounce the shooting - .onTrue(Commands.runOnce(() -> lobbing++)) - .onFalse( - Commands.run(() -> robot.indexer.setFeedToShooter(true)).withTimeout(0.5) - .andThen(Commands.runOnce(() -> { - robot.indexer.setFeedToShooter(false); - lobbing--; - }))); + .whileTrue(Commands.run(() -> robot.shooter.setTempAimCalculator(LobCalculator.INSTANCE) + )); + + new Trigger(() -> driver.leftTrigger.isOutside(0.2)) + .whileTrue(Commands.run(()-> robot.shooter.setTempAimCalculator(LowLobAimCalculator.INSTANCE) + )); new Trigger(this::driverWantsAim).whileTrue(new AimTowardsSpeakerCommand( robot.drive, diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LowLobAimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LowLobAimCalculator.java new file mode 100644 index 0000000..1ebacf8 --- /dev/null +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LowLobAimCalculator.java @@ -0,0 +1,11 @@ +package com.swrobotics.robot.subsystems.speaker.aim; + +public class LowLobAimCalculator implements AimCalculator { + public static final LowLobAimCalculator INSTANCE = new LowLobAimCalculator(); + + @Override + public Aim calculateAim(double distanceToSpeaker) { + return new Aim(40, 21, distanceToSpeaker); + } + +}