From 4a55b146dcebfa9953260d752ddf17ea361b7118 Mon Sep 17 00:00:00 2001 From: Programmers Date: Sat, 4 May 2024 15:04:04 -0500 Subject: [PATCH] Amping up the Amp button --- src/main/java/frc/robot/RobotContainer.java | 47 +++++++++++++++---- .../robot/commands/control/AimLockWrist.java | 2 +- .../frc/robot/commands/control/GoHome.java | 5 +- .../robot/commands/control/IdleShooter.java | 2 +- .../java/frc/robot/subsystems/AmpSensors.java | 8 +++- 5 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e77b1ba..e305bbd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,8 +102,8 @@ public class RobotContainer { new SlewRateLimiter(Constants.translationSlewRate); // private final SlewRateLimiter RotationalSlewRate = // new SlewRateLimiter(Constants.rotationSlewRate); - public static boolean isForwardAmpPrimed = false; - public static boolean isReverseAmpPrimed = false; + public static boolean isAmpPrepped = false; + public static boolean isAmpShot = false; public static boolean isClimbPrimed = false; public static boolean aimAtTargetAuto = false; private double MaxSpeed = DriveConstants.kSpeedAt12VoltsMps; @@ -136,19 +136,46 @@ public RobotContainer() { private void configureDriverController() { DRIVER_CONTROLLER.b().onTrue(new ShootSubwooferFlat(ELEVATOR, WRIST, SHOOTER)); + // DRIVER_CONTROLLER + // .y() + // .onTrue( + // new ConditionalCommand( + // new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE) + // .andThen(() -> isReverseAmpPrimed = false), + // new PrepRevAmp(ELEVATOR, WRIST) + // .andThen(new WaitCommand(0.8)) + // .andThen(new FireRevAmp(SHOOTER)) + // .andThen(new WaitCommand(0.1)) + // .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2))) + // .andThen(new InstantCommand(() -> isReverseAmpPrimed = true)), + // () -> isReverseAmpPrimed)); + DRIVER_CONTROLLER .y() .onTrue( new ConditionalCommand( - new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE) - .andThen(() -> isReverseAmpPrimed = false), + // Compare prepped to not prepped (If prepped, try shoot. Else, prep) + new ConditionalCommand( + // Compare shot to not shot (If shot, go home. If not, try shot) + new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE) + .andThen( + () -> { + isAmpPrepped = false; + isAmpShot = false; + }), + new ConditionalCommand( + // Compare working shot (If working, shoot. Else rumble.) + new FireRevAmp(SHOOTER) + .andThen(new WaitCommand(0.1)) + .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2))) + .andThen(new InstantCommand(() -> isAmpShot = true)), + new AsyncRumble( + DRIVER_CONTROLLER.getHID(), RumbleType.kBothRumble, 1.0, 400L), + () -> AMP_SENSORS.getBothSensors()), + () -> isAmpShot), new PrepRevAmp(ELEVATOR, WRIST) - .andThen(new WaitCommand(0.8)) - .andThen(new FireRevAmp(SHOOTER)) - .andThen(new WaitCommand(0.1)) - .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2))) - .andThen(new InstantCommand(() -> isReverseAmpPrimed = true)), - () -> isReverseAmpPrimed)); + .andThen(new InstantCommand(() -> isAmpPrepped = true)), + () -> isAmpPrepped)); DRIVER_CONTROLLER .back() .onTrue( diff --git a/src/main/java/frc/robot/commands/control/AimLockWrist.java b/src/main/java/frc/robot/commands/control/AimLockWrist.java index cd0e852..1ef1db7 100644 --- a/src/main/java/frc/robot/commands/control/AimLockWrist.java +++ b/src/main/java/frc/robot/commands/control/AimLockWrist.java @@ -33,7 +33,7 @@ public void execute() { if (RobotContainer.isClimbPrimed) { return; } - if (RobotContainer.isForwardAmpPrimed || RobotContainer.isReverseAmpPrimed) { + if (RobotContainer.isAmpPrepped) { return; } // if (Util.canSeeTarget(speakerLimelight)) { diff --git a/src/main/java/frc/robot/commands/control/GoHome.java b/src/main/java/frc/robot/commands/control/GoHome.java index 49a5188..d856bb3 100644 --- a/src/main/java/frc/robot/commands/control/GoHome.java +++ b/src/main/java/frc/robot/commands/control/GoHome.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.RobotContainer; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -23,6 +24,8 @@ public GoHome(Elevator elevator, Wrist wrist, Shooter shooter, Intake intake) { new InstantCommand(wrist::goHome), new InstantCommand(shooter::stopShooter), new InstantCommand(shooter::stopFeeder), - new InstantCommand(intake::stopCollecting)); + new InstantCommand(intake::stopCollecting), + new InstantCommand(() -> RobotContainer.isAmpPrepped = false), + new InstantCommand(() -> RobotContainer.isAmpShot = false)); } } diff --git a/src/main/java/frc/robot/commands/control/IdleShooter.java b/src/main/java/frc/robot/commands/control/IdleShooter.java index 210c5f0..a43028b 100644 --- a/src/main/java/frc/robot/commands/control/IdleShooter.java +++ b/src/main/java/frc/robot/commands/control/IdleShooter.java @@ -39,7 +39,7 @@ public void execute() { shooter.stopShooter(); return; } - if (RobotContainer.isForwardAmpPrimed) { + if (RobotContainer.isAmpPrepped) { shooter.setRPMShoot(Constants.Shooter.SHOOTER_AMP_RPM); return; } diff --git a/src/main/java/frc/robot/subsystems/AmpSensors.java b/src/main/java/frc/robot/subsystems/AmpSensors.java index 8643a75..9be5cb0 100644 --- a/src/main/java/frc/robot/subsystems/AmpSensors.java +++ b/src/main/java/frc/robot/subsystems/AmpSensors.java @@ -18,15 +18,19 @@ public class AmpSensors extends SubsystemBase { public AmpSensors() { m_sensor_left = new DigitalInput(Constants.Wrist.PROXIMITY_SENSOR_LEFT_ID); m_sensor_right = new DigitalInput(Constants.Wrist.PROXIMITY_SENSOR_RIGHT_ID); + Shuffleboard.getTab("MAIN").addBoolean("Amp Left", () -> getSensorLeft()); + Shuffleboard.getTab("MAIN").addBoolean("Amp Rights", () -> getSensorRight()); Shuffleboard.getTab("MAIN").addBoolean("Amp Sensor", () -> getBothSensors()); } public boolean getSensorLeft() { - return m_sensor_left.get(); + //6.2 inches from wall + return !m_sensor_left.get(); } public boolean getSensorRight() { - return m_sensor_right.get(); + //6.2 inches from wall + return !m_sensor_right.get(); } public boolean getBothSensors() {