diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index a6b00b2..9a609c5 100644 --- 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 index 4b892be..4638e7a 100644 --- 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 index 29fc3c2..8e2585b 100644 --- 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 index acd6304..e0117b5 100644 --- 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 index 544cecc..c05c839 100644 --- 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 index b7d692a..bd9a3f0 100644 --- 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 index f2800b6..576280f 100644 --- 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 index 2170ee0..0ffff61 100644 --- 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 index b8386bf..a46a47d 100644 --- 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 index ce644fb..40aa4f3 100644 --- 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 index 4726ffc..cd81000 100644 --- 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 index 126198b..172db01 100644 --- 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 index 64e47dd..8e75301 100644 --- 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 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 index cf41bb5..5eb9fbd 100644 --- 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 index 4a9b17a..3fb94e5 100644 --- 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()) ); }