From e2e86d0aecb052b0be2d636173a2c8f0c1c50ab9 Mon Sep 17 00:00:00 2001
From: Museum25 <91571536+mvog2501@users.noreply.github.com>
Date: Thu, 18 Jul 2024 06:26:35 -0500
Subject: [PATCH] [Robot] Remove driverWantsFlywheels()

driverWantsFlywheels() is not helpful because if the robot doesn't know where it is, the flywheels won't do anything productive anyway.
---
 .../swrobotics/robot/control/ControlBoard.java   | 16 +++++-----------
 1 file changed, 5 insertions(+), 11 deletions(-)

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 2b434c2..df9fd1b 100644
--- a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
+++ b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
@@ -10,7 +10,6 @@
 import com.swrobotics.robot.commands.AimTowardsSpeakerCommand;
 import com.swrobotics.robot.commands.AmpAlignCommand;
 import com.swrobotics.robot.commands.CharactarizeWheelCommand;
-import com.swrobotics.robot.commands.SnapDistanceCommand;
 import com.swrobotics.robot.config.NTData;
 import com.swrobotics.robot.subsystems.amp.AmpArm2Subsystem;
 import com.swrobotics.robot.subsystems.climber.ClimberArm;
@@ -26,7 +25,6 @@
 import com.swrobotics.robot.subsystems.swerve.SwerveDrive.TurnRequest;
 
 import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -122,10 +120,10 @@ public ControlBoard(RobotContainer robot) {
         
         driver.y.onFalling(() -> NTData.SHOOTER_PIVOT_RECALIBRATE.set(true));
 
+        // Intake automatically turns off when we have a game piece
+        robot.intake.set(IntakeSubsystem.State.OFF);
         operator.a.onRising(() -> robot.intake.set(IntakeSubsystem.State.INTAKE));
         operator.a.onFalling(() -> robot.intake.set(IntakeSubsystem.State.OFF));
-        robot.intake.set(IntakeSubsystem.State.OFF);
-
         new Trigger(() -> robot.indexer.hasPiece() || !operator.a.isPressed())
                 .onTrue(Commands.runOnce(() -> robot.intake.set(IntakeSubsystem.State.OFF)));
 
@@ -140,10 +138,6 @@ private boolean driverWantsAim() {
         return driver.rightTrigger.isOutside(TRIGGER_BUTTON_THRESHOLD);
     }
 
-    private boolean driverWantsFlywheels() {
-        return driverFlywheelDebounce.calculate(driver.rightBumper.isPressed());
-    }
-
     private double squareWithSign(double value) {
         double squared = value * value;
         return Math.copySign(squared, value);
@@ -325,7 +319,7 @@ public void periodic() {
         ShooterSubsystem.FlywheelControl flywheelControl = ShooterSubsystem.FlywheelControl.IDLE;
         if (operator.start.isPressed())
             flywheelControl = ShooterSubsystem.FlywheelControl.REVERSE;
-        else if (driverWantsAim() || driverWantsFlywheels() || shootAmp || shootManual || forceToSubwoofer || forceToStageCorner || lobbing != 0)
+        else if (driverWantsAim() || shootAmp || shootManual || forceToSubwoofer || forceToStageCorner || lobbing != 0)
             flywheelControl = ShooterSubsystem.FlywheelControl.SHOOT;
         else if (operatorWantsShoot)
             flywheelControl = ShooterSubsystem.FlywheelControl.POOP;
@@ -337,12 +331,12 @@ else if (operatorWantsShoot)
 
         pieceRumbleNt.set(pieceRumble);
 
-        driver.setRumble(pieceRumble ? 1.0 : (tooFar && (driverWantsAim() || driverWantsFlywheels()) ? 0.5 : 0));
+        driver.setRumble(pieceRumble ? 1.0 : (tooFar && (driverWantsAim()) ? 0.5 : 0));
         boolean shooterReady = robot.shooter.isReadyToShoot();
         operator.setRumble(pieceRumble ? 1.0 : (shooterReady ? 0.5 : 0));
 
         this.shooterReady.set(shooterReady);
-        this.tooFar.set(tooFar && (driverWantsAim() || driverWantsFlywheels()));
+        this.tooFar.set(tooFar && (driverWantsAim()));
     }
 
     NTBoolean pieceRumbleNt = new NTBoolean("Debug/Piece Rumble", false);