Fix: refactored isNoteStaged into isNoteStagedBack

flamingchickens1540 · Jun 12, 2024 · 59e8c52 · 59e8c52
1 parent a09f5b9
commit 59e8c52
Show file tree

Hide file tree

Showing 15 changed files with 26 additions and 22 deletions.
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -208,14 +208,14 @@ private void configureButtonBindings() {
         new Trigger(() -> elevator.getPosition() > 0.1).debounce(0.1)
                 .whileTrue(PrepareShooterCommand.lowerPivot(shooter));
 
-        new Trigger(indexer::isNoteStaged).debounce(0.05)
+        new Trigger(indexer::isNoteStagedBack).debounce(0.05)
                 .onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 1, 1))
                 .whileTrue(leds.commandShowIntakePattern(SimpleLedPattern.solid("#ff0000")));
 
         new Trigger(tramp::isNoteStaged).debounce(0.1)
                 .whileTrue(leds.commandShowTrampPattern(SimpleLedPattern.solid("#ff9900")));
 
-        new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4));
+        new Trigger(indexer::isNoteStagedBack).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4));
 
 
         BooleanSupplier isPreMatch = () -> (!DriverStation.isDSAttached() || !DriverStation.isFMSAttached() || DriverStation.isAutonomous()) && DriverStation.isDisabled();

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADE.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBADE.java
@@ -29,7 +29,7 @@ public CenterLanePCBADE(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                                 Commands.deadline(
                                         Commands.sequence(
                                                 Commands.waitSeconds(0.1),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.1)
                                         ).withTimeout(1.1),
                                         Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer))

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAED.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAED.java
@@ -29,7 +29,7 @@ public CenterLanePCBAED(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                                 Commands.deadline(
                                         Commands.sequence(
                                                 Commands.waitSeconds(0.1),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.1)
                                         ).withTimeout(1.1),
                                         Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer))

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAEF.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAEF.java
@@ -29,7 +29,7 @@ public CenterLanePCBAEF(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                                 Commands.deadline(
                                         Commands.sequence(
                                                 Commands.waitSeconds(0.1),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.1)
                                         ).withTimeout(1.1),
                                         Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer))

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFE.java
@@ -31,7 +31,7 @@ public CenterLanePCBAFE(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                                 Commands.deadline(
                                         Commands.sequence(
                                                 Commands.waitSeconds(0.1),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.1)
                                         ).withTimeout(1.1),
                                         Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer))

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFG.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAFG.java
@@ -29,7 +29,7 @@ public CenterLanePCBAFG(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                                 Commands.deadline(
                                         Commands.sequence(
                                                 Commands.waitSeconds(0.1),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.1)
                                         ).withTimeout(1.1),
                                         Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer))

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAGF.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePCBAGF.java
@@ -29,7 +29,7 @@ public CenterLanePCBAGF(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                                 Commands.deadline(
                                         Commands.sequence(
                                                 Commands.waitSeconds(0.1),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.1)
                                         ).withTimeout(1.1),
                                         Commands.waitSeconds(0.2).andThen(IntakeAndFeed.withDefaults(indexer))

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePDEABC.java
@@ -28,7 +28,7 @@ public CenterLanePDEABC(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                         Commands.sequence(
                                 Commands.deadline(
                                         Commands.sequence(
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.1)
                                         ).withTimeout(1.1),
                                         IntakeAndFeed.withDefaults(indexer)

diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbAlignment.java
@@ -28,7 +28,7 @@ public ClimbAlignment(Drivetrain drivetrain, Elevator elevator, Tramp tramp, Ind
                     new ParallelCommandGroup(
                             new SequentialCommandGroup(
                                     new ElevatorSetpointCommand(elevator, Constants.Elevator.ElevatorState.BOTTOM),
-                                    new StageTrampCommand(tramp, indexer).onlyIf(indexer::isNoteStaged)
+                                    new StageTrampCommand(tramp, indexer).onlyIf(indexer::isNoteStagedBack)
                             ),
                             new ProxyCommand(() -> climbPath(drivetrain::getPose, 1))
                     ),

diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/ContinuousIntakeCommand.java
@@ -35,7 +35,7 @@ public void initialize() {
 
     @Override
     public void execute() {
-        if (indexer.isNoteStaged()) {
+        if (indexer.isNoteStagedBack()) {
             indexer.stopIntake();
             if (leds != null) {
                 leds.clearPatternAll(Leds.PatternLevel.INTAKE_PREREADY);

diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java
@@ -31,7 +31,7 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        return shouldUseBeambreak && (indexer.isNoteStaged());
+        return shouldUseBeambreak && (indexer.isNoteStagedBack());
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/DefaultShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/DefaultShooterCommand.java
@@ -27,16 +27,16 @@ public DefaultShooterCommand(Drivetrain drivetrain, Shooter shooter, Indexer ind
         };
         addCommands(
                 Commands.runOnce(()->shouldShootPrepare = shootPrepareSupplier.getAsBoolean()),
-                Commands.runOnce(()->hasNote = indexer.isNoteStaged()),
+                Commands.runOnce(()->hasNote = indexer.isNoteStagedBack()),
                 Commands.either(
                         Commands.either(
                                 new LeadingShootPrepare(drivetrain, shooter, Constants.Shooter.Flywheels.LEFT_RPM*0.75, Constants.Shooter.Flywheels.RIGHT_RPM*0.75).alongWith(Commands.runOnce(()->hasNote = true)),
                                 new LeadingShootPrepare(drivetrain, shooter, 0, 0).alongWith(Commands.runOnce(()->hasNote = true)),
-                                indexer::isNoteStaged
+                                indexer::isNoteStagedBack
                         ).alongWith(Commands.runOnce(()-> shouldShootPrepare = true)),
                         new OverStageShootPrepare(drivetrain::getPose, shooter, 0, 0).alongWith(Commands.runOnce(()-> shouldShootPrepare = false)),
                         shootPrepareSupplier
-                ).until(()-> shootPrepareSupplier.getAsBoolean() != shouldShootPrepare || indexer.isNoteStaged() != hasNote).repeatedly()
+                ).until(()-> shootPrepareSupplier.getAsBoolean() != shouldShootPrepare || indexer.isNoteStagedBack() != hasNote).repeatedly()
         );
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java
@@ -37,8 +37,8 @@ public TuneShooterCommand(Shooter shooter, Indexer indexer, Supplier<Pose2d> pos
                                 Rotation2d.fromDegrees(angleSetpoint.get()), leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()
                         )),
                 Commands.sequence(
-                        Commands.waitUntil(indexer::isNoteStaged),
-                        Commands.waitUntil(()->!indexer.isNoteStaged()),
+                        Commands.waitUntil(indexer::isNoteStagedBack),
+                        Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                         Commands.waitSeconds(1),
                         Commands.runOnce(()->shotNum += 1),
                         Commands.print("Shot Number: " + shotNum + " Angle Degrees Setpoint: " + pivotRotationSupplier.get() + " Left RPM Setpoint: " + shooterLeftSupplier.getAsDouble() + " Right RPM Setpoint: " + shooterRightSupplier.getAsDouble() +

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java
@@ -16,7 +16,9 @@
 public class IndexerIOTalonFX implements IndexerIO {
     private final TalonFX intakeMotor = new TalonFX(INTAKE_ID);
     private final TalonFX feederMotor = new TalonFX(FEEDER_ID);
-    private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
+    private final DigitalInput indexerBeamBreakFront = new DigitalInput(BEAM_BREAK_FRONT_ID);
+    private final DigitalInput indexerBeamBreakBack = new DigitalInput(BEAM_BREAK_BACK_ID);
+    private final DigitalInput indexerBeamBreakShooter = new DigitalInput(BEAM_BREAK_SHOOTER_ID);
 
     private final VoltageOut feederVoltageCtrlReq = new VoltageOut(0).withEnableFOC(true);
     private final VelocityVoltage feederVelocityCtrlReq = new VelocityVoltage(0).withEnableFOC(true);
@@ -87,7 +89,9 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.feederCurrentAmps = feederCurrent.getValueAsDouble();
         inputs.feederVelocityRPM = feederVelocity.getValueAsDouble() * 60;
         inputs.feederTempCelsius = feederTemp.getValueAsDouble();
-        inputs.noteInIntake = !indexerBeamBreak.get();
+        inputs.noteInIntakeFront = !indexerBeamBreakFront.get();
+        inputs.noteInIntakeBack = !indexerBeamBreakBack.get();
+        inputs.noteInIntakeShooter = !indexerBeamBreakShooter.get();
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java
@@ -103,15 +103,15 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter,
 //                                Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw),
                                 new ParallelDeadlineGroup(
                                         Commands.sequence(
-                                                Commands.waitUntil(indexer::isNoteStaged),
+                                                Commands.waitUntil(indexer::isNoteStagedBack),
                                                 Commands.waitSeconds(extraPreShotWait),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                                Commands.waitUntil(()->!indexer.isNoteStagedBack()),
                                                 Commands.waitSeconds(0.2 - postShotWaitReduction)
                                         ).withTimeout(1+extraPreShotWait),
                                         IntakeAndFeed.withDefaults(indexer)
                                 )
                         )
-                ).unless(() -> shouldSkipIfEmpty && !indexer.isNoteStaged())
+                ).unless(() -> shouldSkipIfEmpty && !indexer.isNoteStagedBack())
         );
     }