From 8c4d703dd26963aad59e037ff6c60363300788ee Mon Sep 17 00:00:00 2001 From: Saarth Date: Tue, 24 Mar 2026 20:20:43 -0400 Subject: [PATCH 1/7] cleaned up superstructure --- src/main/java/frc/robot/Robot.java | 10 +- .../robot/autons/WaltSimpleAutonFactory.java | 6 +- .../java/frc/robot/subsystems/Intake.java | 9 +- .../frc/robot/subsystems/Superstructure.java | 272 +----------------- 4 files changed, 31 insertions(+), 266 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 995f7f19..f81e4ac9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -275,7 +275,7 @@ private void configureBindings() { //Shooting // NORMAL FIXED SHOT // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - trg_shoot.whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above + trg_shoot.whileTrue(m_superstructure.shootWithShotCalc()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); @@ -298,7 +298,7 @@ private void configureBindings() { m_superstructure.emergencyBarf() ); - trg_shimmy.whileTrue(m_superstructure.shimmy()); + trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); trg_unjam.and(trg_shoot.negate()).whileTrue( m_superstructure.unjamCmd(() -> false) @@ -315,12 +315,12 @@ private void configureBindings() { // m_manipulator.a().and(trg_manipOverride).onTrue(m_shooter.setHoodPositionCmd(Degrees.of(1))); trg_deployIntakeOverride.onTrue( - m_superstructure.intakeTo(IntakeArmPosition.DEPLOYED) + m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED) ).onFalse( - m_superstructure.intakeTo(IntakeArmPosition.SAFE) + m_intake.setIntakeArmPosCmd(IntakeArmPosition.SAFE) ); trg_intakeUpOverride.onTrue( - m_superstructure.intakeTo(IntakeArmPosition.RETRACTED) + m_intake.setIntakeArmPosCmd(IntakeArmPosition.RETRACTED) ); // m_driver.y().and(trg_driverOverride).onTrue(m_shooter.turretHomingCmd(false)); //false? im not sure diff --git a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java index 254d64e4..56f0c7fd 100644 --- a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java @@ -72,7 +72,7 @@ private Command homingCmd() { private Command shootWithTimeout(AngularVelocity speed, double seconds) { return Commands.sequence( tp("shootWithTimeout.START(" + speed + "," + seconds + ")"), - m_superstructure.activateOuttakeShotCalc(), //(() -> speed).withTimeout(seconds), + m_superstructure.shootWithShotCalc(), //(() -> speed).withTimeout(seconds), tp("shootWithTimeout.END") ); } @@ -153,7 +153,7 @@ public AutoRoutine preloadAuton() { routine.active().onTrue( homingCmd().andThen( - m_superstructure.activateOuttakeShotCalc() + m_superstructure.shootWithShotCalc() ) ); @@ -261,7 +261,7 @@ public AutoRoutine fastTwoSweep_Reshoot(boolean left) { ); traj1.doneFor(AutonK.kShootingTimeout).whileTrue( - m_superstructure.activateOuttakeShotCalc() + m_superstructure.shootWithShotCalc() ); traj1.doneDelayed(AutonK.kShootingTimeout + 0.04).onTrue(traj2.cmd()); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0a880d2b..ed0c607f 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -128,7 +128,14 @@ public boolean isIntakeArmAtPos() { return isNear; } - public Command shimmy() { + public Command retractIntakeArmCmd() { + return Commands.sequence( + stopIntakeRollers(), + setIntakeArmPosCmd(IntakeArmPosition.RETRACTED) + ); + } + + public Command intakeArmShimmy() { return Commands.repeatingSequence( setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), setIntakeRollersVelocityCmd(0), diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ae75f462..c8b4418f 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,26 +1,15 @@ - package frc.robot.subsystems; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import frc.robot.Constants.IndexerK; -import frc.robot.Constants.ShooterK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.Turret; -import frc.util.WaltLogger; -import frc.util.WaltLogger.StringArrayLogger; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.Constants.SuperstructureK.*; import static frc.robot.Constants.ShooterK; -import static frc.robot.Constants.IntakeK; -import java.util.HashSet; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -31,13 +20,7 @@ public class Superstructure extends SubsystemBase { private final Shooter m_shooter; /* LOGGERS */ - private HashSet m_activeCommands = new HashSet<>(); - private final StringArrayLogger log_activeCommands = WaltLogger.logStringArray(kLogTab, "Active Commands"); - - private HashSet m_activeOverrideCommands = new HashSet<>(); - private final StringArrayLogger log_activeOverrideCommands = WaltLogger.logStringArray(kLogTab, "Active Override Commands"); - // private StringLogger log_shooterState = WaltLogger.logString(kLogTab, "ShooterState"); /* CONSTRUCTOR */ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { @@ -46,10 +29,11 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { m_shooter = shooter; } - /* BUTTON BIND SEQUENCES */ + /* SUBSYSTEM COMMANDS */ + + //---INTAKE COMMANDS /** - * * @param isShooting * @return */ @@ -77,22 +61,13 @@ public Command intake(BooleanSupplier isShooting) { ); } - public Command startShootSequenceNOSHOOT() { - return Commands.parallel( - Commands.sequence( - m_indexer.startTunnelCmd(), - m_indexer.startSpindexerCmd() - ) - ); + public Command retractIntakeCmd() { + return m_intake.retractIntakeArmCmd(); } - /** - * @param update message to update the SHOOTER logger - * @return a Command that updates the logger - */ - // public Command up(String update) { - // return Commands.runOnce(() -> log_shooterState.accept(update)); - // } + public Command intakeArmShimmy() { + return m_intake.intakeArmShimmy(); + } /** * Turns on spinner and exhaust and sets shooter speed to RPS. @@ -100,41 +75,25 @@ public Command startShootSequenceNOSHOOT() { * Note: does not move turret or hood. * @param RPS the speed for the shooter */ - public Command activateOuttake(Supplier RPS) { - // log_shooterState.accept("pre sequence"); + public Command shoot(Supplier RPS) { return Commands.parallel( - // up("post sequence call"), m_shooter.setShooterVelocityCmdSupp(RPS), - // up("post supplier command"), Commands.sequence( - // up("pre waituntil command"), Commands.waitUntil(() -> (m_shooter.isShooterSpunUp() && (m_shooter.getShooterVelocity().gte(ShooterK.kShooterSpunUpMinimum)))).withTimeout(ShooterK.kShooterSpunUpTimeout), m_indexer.startTunnelCmd(), Commands.waitUntil(() -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum))).withTimeout(IndexerK.kTunnelSpunUpTimeout), - // up("post waituntil command"), - // up("pre start indexer"), m_indexer.startSpindexerCmd(), - // up("post start indexer"), - // up("pre repeating sequence"), Commands.repeatingSequence( - // up("in repeating sequence"), - // m_indexer.stopIndexerCmd() - // .onlyIf(() -> !m_shooter.isShooterSpunUp()) - // .andThen(m_indexer.startIndexerCmd()).beforeStarting(Commands.waitUntil(() -> m_shooter.isShooterSpunUp())) - // up("end repeating sequence") - // Commands.print("shotCalc cope sequence: INITIATED") Commands.none() ) ) - // up("post repeating sequence") ) .finallyDo( () -> { - deactivateOuttake(); - // log_shooterState.accept("post deactivate outtake"); + stopShooting(); } ); } @@ -145,68 +104,41 @@ public Command activateOuttake(Supplier RPS) { * Note: does not move turret or hood. * @param RPS the speed for the shooter */ - public Command activateOuttakeShotCalc() { - // log_shooterState.accept("pre sequence"); + public Command shootWithShotCalc() { return Commands.parallel( - // up("post sequence call"), m_shooter.shootFromCalc(), - // up("post supplier command"), Commands.sequence( - // up("pre waituntil command"), Commands.waitUntil(() -> (m_shooter.isShooterSpunUp() && (m_shooter.getShooterVelocity().gte(ShooterK.kShooterSpunUpMinimum)))).withTimeout(ShooterK.kShooterSpunUpTimeout), m_indexer.startTunnelCmd(), Commands.waitUntil(() -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum))).withTimeout(IndexerK.kTunnelSpunUpTimeout), - // up("post waituntil command"), - // up("pre start indexer"), m_indexer.startSpindexerCmd() - // up("post start indexer"), ) ) .finallyDo( () -> { - deactivateOuttake(); - // log_shooterState.accept("post deactivate outtake"); + stopShooting(); } ); } - public Command activateOuttakeNOSHOOT() { - return Commands.parallel( - startShootSequenceNOSHOOT() - ).finallyDo( - () -> deactivateOuttakeNOSHOOT() - ); - } - /** * Turns off spinner, exhaust, and shooter. *

* Note: does not move turret or hood. */ - public void deactivateOuttake() { + public void stopShooting() { m_indexer.stopSpindexer(); m_indexer.stopTunnel(); m_shooter.setShooterVelocity(ShooterK.kShooterZeroRPS); - // m_shooter.setHoodPosition(Degrees.of(1)); - - // Commands.sequence(logActiveCommands("deactivateOuttake", "shooting", "emergencyDump")); - } - - public void deactivateOuttakeNOSHOOT() { - m_indexer.stopSpindexer(); - m_indexer.stopTunnel(); - // m_shooter.setHoodPosition(Degrees.of(1)); - // Commands.sequence(logActiveCommands("deactivateOuttake", "shooting", "emergencyDump")); } public Command emergencyBarf() { return Commands.startEnd( () -> { m_intake.setIntakeArmPos(IntakeArmPosition.DEPLOYED); - // m_shooter.setHoodPosition(ShooterK.kHoodMaxDegs); m_indexer.setTunnelVelocity(IndexerK.kTunnelShootRPS); m_indexer.setSpindexerVelocity(IndexerK.kSpindexerShootRPS); m_shooter.setShooterVelocity(ShooterK.kShooterBarfRPS); @@ -216,64 +148,13 @@ public Command emergencyBarf() { m_intake.setIntakeRollersVelocity(0); m_intake.setIntakeArmPos(IntakeArmPosition.SAFE); m_shooter.setShooterVelocity(RotationsPerSecond.zero()); - // m_shooter.setHoodPosition(ShooterK.kHoodSafeDegs); m_indexer.setSpindexerVelocity(RotationsPerSecond.zero()); m_indexer.setTunnelVelocity(RotationsPerSecond.zero()); } ); } - public Command shimmy() { - return m_intake.shimmy(); - } - - /** - * Rotates the turret to the given degs - * @param degs degrees to rotate to. - */ - public Command turretTo(Angle degs) { - // Command logCommand; - // if (degs.magnitude() == 180) { - // logCommand = logActiveOverrideCommands("turret180", "turret0"); - // } else { - // logCommand = logActiveOverrideCommands("turret0", "turret180"); - // } - return Commands.sequence( - m_shooter.m_turret.setTurretPosCmd(Rotations.of(degs.in(Rotations))) - // logCommand - ); - } - - /** - * Raises/lowers the hood to the given degs. - * @param degs degrees to raise/lower to. - * @return - */ - public Command hoodTo(Angle degs) { - return Commands.sequence( - // m_shooter.setHoodPositionCmd(degs) - ); - } - - /** - * Starts the indexer spinner. - */ - public Command startSpindexerCmd() { - return Commands.sequence( - m_indexer.startSpindexerCmd() - // logActiveOverrideCommands("startSpindexerCmd", "stopSpindexerCmd") - ); - } - - /** - * Stops the indexer spinner. - */ - public Command stopSpindexerCmd() { - return Commands.sequence( - m_indexer.stopSpindexerCmd() - // logActiveOverrideCommands("stopSpindexerCmd", "startSpindexerCmd") - ); - } + public Command unjamCmd(BooleanSupplier isShooting) { return Commands.runEnd( @@ -296,127 +177,4 @@ else if (isShooting.getAsBoolean()) { } ); } - - /** - * Starts the indexer exhaust. - */ - public Command startTunnelCmd() { - return Commands.sequence( - m_indexer.startTunnelCmd() - // logActiveOverrideCommands("startTunnelCmd", "stopTunnelCmd") - ); - } - - /** - * Stops the indexer exhaust. - */ - public Command stopTunnelCmd() { - return Commands.sequence( - m_indexer.stopTunnelCmd() - // logActiveOverrideCommands("stopTunnelCmd", "startTunnelCmd") - ); - } - - /** - * Starts the intake rollers. - */ - public Command startIntakeRollers() { - return Commands.sequence( - m_intake.startIntakeRollers() - // logActiveOverrideCommands("startIntakeRollers", "stopIntakeRollers") - ); - } - - /** - * Stops the intake rollers. - */ - public Command stopIntakeRollers() { - return Commands.sequence( - m_intake.stopIntakeRollers() - // logActiveOverrideCommands("stopIntakeRollers", "startIntakeRollers") - ); - } - - /** - * Deploys the intake to the given pos. - * @param pos position to deploy to. - * @return - */ - public Command intakeTo(IntakeArmPosition pos) { - // Command logCommand; - // switch (pos) { - // case DEPLOYED: - // logCommand = logActiveOverrideCommands("deployIntake", "safeIntake", "intakeUp"); - // break; - // case SAFE: - // logCommand = logActiveOverrideCommands("safeIntake", "deployIntake", "intakeUp"); - // break; - // default: - // if (m_intake.getIntakeArmStatorCurrent() < 40) { - // logCommand = logActiveOverrideCommands("intakeUp", "safeIntake", "deployIntake"); - // } - // else { - // return Commands.none(); - // } - // break; - // } - return Commands.sequence( - m_intake.setIntakeArmPosCmd(pos) - // logCommand - ); - } - - // /** - // * Adds and removes specified Command names from the ActiveCommands ArrayList, then logs the ArrayList. - // * @param toAdd Command name to add. - // * @param toRemove Command names to remove. - // */ - // private Command logActiveCommands(String toAdd, String... toRemove) { - // Command addTo = Commands.runOnce( - // () -> m_activeCommands.add(toAdd) - // ); - // Command removeFrom = Commands.runOnce( - // () -> { - // for (String s : toRemove) { - // m_activeCommands.remove(s); - // } - // } - // ); - // Command updateLog = Commands.runOnce( - // () -> log_activeCommands.accept(m_activeCommands.toArray(new String[m_activeCommands.size()])) - // ); - - // return Commands.sequence( - // addTo, - // removeFrom, - // updateLog - // ); - // } - - // /** - // * Adds and removes specified override Command names from the activeOverridesCommands ArrayList, then logs the ArrayList. - // * @param toAdd override Command name to add. - // * @param toRemove override Command names to remove. - // */ - // private Command logActiveOverrideCommands(String toAdd, String... toRemove) { - // Command addTo = Commands.runOnce( - // () -> m_activeOverrideCommands.add(toAdd) - // ); - // Command removeFrom = Commands.runOnce( - // () -> { - // for (String s : toRemove) { - // m_activeOverrideCommands.remove(s); - // } - // } - // ); - // Command updateLog = Commands.runOnce( - // () -> log_activeOverrideCommands.accept(m_activeOverrideCommands.toArray(new String[m_activeOverrideCommands.size()])) - // ); - - // return Commands.sequence( - // addTo, - // removeFrom, - // updateLog - // ); - // } -} +} \ No newline at end of file From 5775912b8068392a0d5e6952d733c3e8049218fb Mon Sep 17 00:00:00 2001 From: Saarth Date: Tue, 24 Mar 2026 20:43:55 -0400 Subject: [PATCH 2/7] renamed methods to specify Cmd and added javadocs --- src/main/java/frc/robot/Robot.java | 10 ++-- .../robot/autons/WaltSimpleAutonFactory.java | 18 +++---- .../frc/robot/subsystems/Superstructure.java | 48 +++++++++++-------- 3 files changed, 42 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f81e4ac9..1410faef 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -265,7 +265,7 @@ private void configureBindings() { //---NORMAL SEQUENCES //Intake trg_intake.and(trg_shoot.negate()).whileTrue( - m_superstructure.intake(() -> false) + m_superstructure.intakeCmd(() -> false) ); trg_retractIntake.onTrue( @@ -275,7 +275,7 @@ private void configureBindings() { //Shooting // NORMAL FIXED SHOT // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - trg_shoot.whileTrue(m_superstructure.shootWithShotCalc()); //comment out for LERP with above + trg_shoot.whileTrue(m_superstructure.shootWithShotCalcCmd()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); @@ -291,14 +291,14 @@ private void configureBindings() { trg_shoot.onTrue(WaltCamera.takeSnapshotCmd()); trg_intake.and(trg_shoot).whileTrue( - m_superstructure.intake(() -> true) + m_superstructure.intakeCmd(() -> true) ); trg_emergencyBarf.whileTrue( - m_superstructure.emergencyBarf() + m_superstructure.emergencyBarfCmd() ); - trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); + trg_shimmy.whileTrue(m_superstructure.intakeArmShimmyCmd()); trg_unjam.and(trg_shoot.negate()).whileTrue( m_superstructure.unjamCmd(() -> false) diff --git a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java index 56f0c7fd..1a435cf1 100644 --- a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java @@ -72,7 +72,7 @@ private Command homingCmd() { private Command shootWithTimeout(AngularVelocity speed, double seconds) { return Commands.sequence( tp("shootWithTimeout.START(" + speed + "," + seconds + ")"), - m_superstructure.shootWithShotCalc(), //(() -> speed).withTimeout(seconds), + m_superstructure.shootWithShotCalcCmd(), //(() -> speed).withTimeout(seconds), tp("shootWithTimeout.END") ); } @@ -126,7 +126,7 @@ public AutoRoutine firstSweep_NoPreload(boolean left) { // Homing then intake in parallel with trajectory (no .asProxy() needed) routine.active().onTrue( homingCmd().andThen( - m_superstructure.intake(() -> false).withTimeout(AutonK.kIntakeTimeout) + m_superstructure.intakeCmd(() -> false).withTimeout(AutonK.kIntakeTimeout) ) ); @@ -153,7 +153,7 @@ public AutoRoutine preloadAuton() { routine.active().onTrue( homingCmd().andThen( - m_superstructure.shootWithShotCalc() + m_superstructure.shootWithShotCalcCmd() ) ); @@ -170,7 +170,7 @@ public AutoRoutine fastOneSweep(boolean left) { routine.active().onTrue( homingCmd().andThen( - m_superstructure.intake(() -> false).withTimeout(3.5) + m_superstructure.intakeCmd(() -> false).withTimeout(3.5) ) ); @@ -196,7 +196,7 @@ public AutoRoutine fastRightTwoSweep_ZigZag() { routine.active().onTrue( homingCmd().andThen( - m_superstructure.intake(() -> false).withTimeout(3.5) + m_superstructure.intakeCmd(() -> false).withTimeout(3.5) ) ); @@ -213,7 +213,7 @@ public AutoRoutine fastRightTwoSweep_ZigZag() { // Phase 2: intake during zigzag (no final shoot) traj2.atTime(2).onTrue( - m_superstructure.intake(() -> false).withTimeout(10) + m_superstructure.intakeCmd(() -> false).withTimeout(10) ); traj2.done().onTrue(m_swerve.xBrakeCmd()); @@ -242,7 +242,7 @@ public AutoRoutine fastTwoSweep_Reshoot(boolean left) { routine.active().onTrue( homingCmd().andThen( - m_superstructure.intake(() -> false) + m_superstructure.intakeCmd(() -> false) ).until(stopIntk1Trg) ); @@ -261,14 +261,14 @@ public AutoRoutine fastTwoSweep_Reshoot(boolean left) { ); traj1.doneFor(AutonK.kShootingTimeout).whileTrue( - m_superstructure.shootWithShotCalc() + m_superstructure.shootWithShotCalcCmd() ); traj1.doneDelayed(AutonK.kShootingTimeout + 0.04).onTrue(traj2.cmd()); // Phase 2: intake during reshoot path startIntk2Trg.onTrue( - m_superstructure.intake(() -> false) //.until(stopIntk2Trg) + m_superstructure.intakeCmd(() -> false) //.until(stopIntk2Trg) ); // Final: xBrake + shoot after traj2 diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index c8b4418f..9fb59cdc 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -34,10 +34,10 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { //---INTAKE COMMANDS /** - * @param isShooting - * @return + * @param isShooting if the robot is currently shooting + * @return a Command that deploys the intake arm and runs the overall intaking logic */ - public Command intake(BooleanSupplier isShooting) { + public Command intakeCmd(BooleanSupplier isShooting) { return Commands.sequence( m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), Commands.waitUntil(() -> m_intake.isIntakeArmAtPos()).withTimeout(0.25), @@ -61,21 +61,27 @@ public Command intake(BooleanSupplier isShooting) { ); } + /** + * @return a Command that turns off the intake rollers and puts the intake arm in the RETRACTED position + */ public Command retractIntakeCmd() { return m_intake.retractIntakeArmCmd(); } - public Command intakeArmShimmy() { + /** + * @return a Command that calls intakeArmShimmy (oscillates the intakeArm between the DEPLOYED and SHIMMY position) + */ + public Command intakeArmShimmyCmd() { return m_intake.intakeArmShimmy(); } + //---SHOOTING COMMANDS + /** - * Turns on spinner and exhaust and sets shooter speed to RPS. - *

- * Note: does not move turret or hood. - * @param RPS the speed for the shooter + * @param RPS Supplier for the desired shooter speed + * @return a Command that starts and controls the overall shooting sequence logic */ - public Command shoot(Supplier RPS) { + public Command shootCmd(Supplier RPS) { return Commands.parallel( m_shooter.setShooterVelocityCmdSupp(RPS), @@ -99,12 +105,9 @@ public Command shoot(Supplier RPS) { } /** - * Turns on spinner and exhaust and sets shooter speed to CALCULATE SHOT RPS - *

- * Note: does not move turret or hood. - * @param RPS the speed for the shooter + * @return a Command that starts and controls the overall shooting sequence logic using the shotClauclator value */ - public Command shootWithShotCalc() { + public Command shootWithShotCalcCmd() { return Commands.parallel( m_shooter.shootFromCalc(), @@ -124,9 +127,7 @@ public Command shootWithShotCalc() { } /** - * Turns off spinner, exhaust, and shooter. - *

- * Note: does not move turret or hood. + * Stops shooting by turning off the spindexer, tunnel, and shooter */ public void stopShooting() { m_indexer.stopSpindexer(); @@ -135,7 +136,12 @@ public void stopShooting() { } - public Command emergencyBarf() { + //---COPE COMMANDS + + /** + * @return a Command that ejects fuel from the intake and shooter while active + */ + public Command emergencyBarfCmd() { return Commands.startEnd( () -> { m_intake.setIntakeArmPos(IntakeArmPosition.DEPLOYED); @@ -154,8 +160,10 @@ public Command emergencyBarf() { ); } - - + /** + * @param isShooting if the shooter is currently shooting + * @return a Command that unjams the shooter by running the spindexer, tunnel, and shooter (if it is running) backwards + */ public Command unjamCmd(BooleanSupplier isShooting) { return Commands.runEnd( () -> { From f73c4336dfbc7d96a4c08299f329dc0d81a19b3e Mon Sep 17 00:00:00 2001 From: Saarth Date: Wed, 25 Mar 2026 19:09:41 -0400 Subject: [PATCH 3/7] !!IN PROGRESS!! committing bc alex wants to see it so bad --- .../java/frc/robot/subsystems/Intake.java | 20 +++++++++++---- .../frc/robot/subsystems/Superstructure.java | 25 ++++++++++++++++++- 2 files changed, 39 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index ed0c607f..cb4c02c8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -121,11 +121,15 @@ public void setIntakeArmPos(Angle rots, double RPSPS) { m_intakeArm.setControl(m_MMVReq.withPosition(rots).withAcceleration(RPSPS)); } - public boolean isIntakeArmAtPos() { + public boolean intakeArmAtTargetPos(double tolerance) { var err = m_intakeArm.getClosedLoopError(); log_intakeArmClosedLoopError.accept(err.getValueAsDouble()); - boolean isNear = m_intakeArm.getClosedLoopError().isNear(0, 0.01); - return isNear; + return err.isNear(0, tolerance); + } + + public boolean intakeArmAtSpecifiedPos(IntakeArmPosition pos, double tolerance) { + double err = Math.abs(pos.rots.minus(m_intakeArm.getPosition().getValue()).in(Rotations)); + return err <= tolerance; } public Command retractIntakeArmCmd() { @@ -140,9 +144,9 @@ public Command intakeArmShimmy() { setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), setIntakeRollersVelocityCmd(0), setIntakeArmPosCmd(IntakeArmPosition.SHIMMY), - Commands.waitUntil(() -> isIntakeArmAtPos()), + Commands.waitUntil(() -> intakeArmAtTargetPos(0.01)), setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), - Commands.waitUntil(() -> isIntakeArmAtPos()) + Commands.waitUntil(() -> intakeArmAtTargetPos(0.01)) ).finallyDo(() -> setIntakeArmPosCmd(IntakeArmPosition.SAFE)); } @@ -171,6 +175,12 @@ public Command setIntakeRollersVelocityCmd(double volts) { return runOnce(() -> setIntakeRollersVelocity(volts)); } + public boolean rollersAtMaxSpeed(double tolerance) { + var err = m_intakeRollers.getClosedLoopError(); + log_intakeArmClosedLoopError.accept(err.getValueAsDouble()); + return err.isNear(0, tolerance); + } + public void setIntakeFlapServo(double pos) { m_intakeFlapServo.setAngle(pos); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9fb59cdc..a0b4cf10 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -19,6 +19,18 @@ public class Superstructure extends SubsystemBase { private final Indexer m_indexer; private final Shooter m_shooter; + /*TRUTH MACHINE SUPPLIERS*/ + private BooleanSupplier supp_intakeArmIsDeployed; + private BooleanSupplier supp_intakeArmIsSafe; + private BooleanSupplier supp_intakeArmIsShimmying; + private BooleanSupplier supp_isIntaking; + + private BooleanSupplier supp_turretIsLocked; + private BooleanSupplier supp_isShooting; + + private BooleanSupplier supp_isBarfing; + private BooleanSupplier supp_isUnjamming; + /* LOGGERS */ @@ -27,6 +39,17 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { m_intake = intake; m_indexer = indexer; m_shooter = shooter; + + supp_intakeArmIsDeployed = () -> m_intake.intakeArmAtSpecifiedPos(IntakeArmPosition.DEPLOYED, 0.01); + supp_intakeArmIsSafe = () -> m_intake.intakeArmAtSpecifiedPos(IntakeArmPosition.SAFE, 0.01); + supp_intakeArmIsShimmying = () -> false; + supp_isIntaking = () -> (supp_intakeArmIsDeployed.getAsBoolean() && m_intake.rollersAtMaxSpeed(5)); + + supp_turretIsLocked = () -> false; + supp_isShooting = () -> false; + + supp_isBarfing = () -> false; + supp_isUnjamming = () -> false; } /* SUBSYSTEM COMMANDS */ @@ -40,7 +63,7 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { public Command intakeCmd(BooleanSupplier isShooting) { return Commands.sequence( m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), - Commands.waitUntil(() -> m_intake.isIntakeArmAtPos()).withTimeout(0.25), + Commands.waitUntil(() -> m_intake.intakeArmAtTargetPos(0.01)).withTimeout(0.25), Commands.run( () -> { boolean shooting = isShooting.getAsBoolean(); From 3bba2d8af5fdddea390bfb8921a1f8bcd493900b Mon Sep 17 00:00:00 2001 From: Saarth Date: Wed, 25 Mar 2026 20:08:37 -0400 Subject: [PATCH 4/7] added suppliers might be cope but i'll fix ts later --- src/main/java/frc/robot/Robot.java | 12 ++--- .../frc/robot/subsystems/Superstructure.java | 47 ++++++++++++------- 2 files changed, 34 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1410faef..650de07b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -300,12 +300,8 @@ private void configureBindings() { trg_shimmy.whileTrue(m_superstructure.intakeArmShimmyCmd()); - trg_unjam.and(trg_shoot.negate()).whileTrue( - m_superstructure.unjamCmd(() -> false) - ); - - trg_unjam.and(trg_shoot).whileTrue( - m_superstructure.unjamCmd(()-> true) + trg_unjam.whileTrue( + m_superstructure.unjamCmd(() -> trg_shoot.getAsBoolean()) ); //---OVERRIDE COMMANDS @@ -325,8 +321,8 @@ private void configureBindings() { // m_driver.y().and(trg_driverOverride).onTrue(m_shooter.turretHomingCmd(false)); //false? im not sure - m_driver.povDown().onTrue(m_shooter.m_turret.setTurretLockCmd(false)); - m_driver.povRight().onTrue(m_shooter.m_turret.setTurretLockCmd(true)); + m_driver.povDown().onTrue(m_superstructure.lockTurret()); + m_driver.povRight().onTrue(m_superstructure.unlockTurret()); // m_driver.start().whileTrue(m_superstructure.activateOuttakeNOSHOOT()); // trg_optimalPrefireTime.whileTrue( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a0b4cf10..c9c02a51 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -20,10 +20,13 @@ public class Superstructure extends SubsystemBase { private final Shooter m_shooter; /*TRUTH MACHINE SUPPLIERS*/ - private BooleanSupplier supp_intakeArmIsDeployed; - private BooleanSupplier supp_intakeArmIsSafe; - private BooleanSupplier supp_intakeArmIsShimmying; - private BooleanSupplier supp_isIntaking; + private BooleanSupplier supp_intakeArmDeployed; + private BooleanSupplier supp_intakeArmSafe; + private BooleanSupplier supp_intakeArmShimmying; + private BooleanSupplier supp_intaking; + + private BooleanSupplier supp_shooterReadyToShoot; + private BooleanSupplier supp_tunnelReadyToShoot; private BooleanSupplier supp_turretIsLocked; private BooleanSupplier supp_isShooting; @@ -40,16 +43,19 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { m_indexer = indexer; m_shooter = shooter; - supp_intakeArmIsDeployed = () -> m_intake.intakeArmAtSpecifiedPos(IntakeArmPosition.DEPLOYED, 0.01); - supp_intakeArmIsSafe = () -> m_intake.intakeArmAtSpecifiedPos(IntakeArmPosition.SAFE, 0.01); - supp_intakeArmIsShimmying = () -> false; - supp_isIntaking = () -> (supp_intakeArmIsDeployed.getAsBoolean() && m_intake.rollersAtMaxSpeed(5)); + supp_intakeArmDeployed = () -> m_intake.intakeArmAtSpecifiedPos(IntakeArmPosition.DEPLOYED, 0.01); + supp_intakeArmSafe = () -> m_intake.intakeArmAtSpecifiedPos(IntakeArmPosition.SAFE, 0.01); + supp_intakeArmShimmying = () -> intakeArmShimmyCmd().isScheduled(); + supp_intaking = () -> (supp_intakeArmDeployed.getAsBoolean() && m_intake.rollersAtMaxSpeed(5)); + + supp_shooterReadyToShoot = () -> (m_shooter.isShooterSpunUp() && (m_shooter.getShooterVelocity().gte(ShooterK.kShooterSpunUpMinimum))); + supp_tunnelReadyToShoot = () -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum)); - supp_turretIsLocked = () -> false; - supp_isShooting = () -> false; + supp_turretIsLocked = () -> m_shooter.m_turret.getTurretLocked(); + supp_isShooting = () -> supp_shooterReadyToShoot.getAsBoolean() && supp_tunnelReadyToShoot.getAsBoolean(); - supp_isBarfing = () -> false; - supp_isUnjamming = () -> false; + supp_isBarfing = () -> emergencyBarfCmd().isScheduled(); + supp_isUnjamming = () -> unjamCmd(supp_isShooting).isScheduled(); } /* SUBSYSTEM COMMANDS */ @@ -109,9 +115,9 @@ public Command shootCmd(Supplier RPS) { m_shooter.setShooterVelocityCmdSupp(RPS), Commands.sequence( - Commands.waitUntil(() -> (m_shooter.isShooterSpunUp() && (m_shooter.getShooterVelocity().gte(ShooterK.kShooterSpunUpMinimum)))).withTimeout(ShooterK.kShooterSpunUpTimeout), + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), m_indexer.startTunnelCmd(), - Commands.waitUntil(() -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum))).withTimeout(IndexerK.kTunnelSpunUpTimeout), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), m_indexer.startSpindexerCmd(), @@ -135,9 +141,9 @@ public Command shootWithShotCalcCmd() { m_shooter.shootFromCalc(), Commands.sequence( - Commands.waitUntil(() -> (m_shooter.isShooterSpunUp() && (m_shooter.getShooterVelocity().gte(ShooterK.kShooterSpunUpMinimum)))).withTimeout(ShooterK.kShooterSpunUpTimeout), + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), m_indexer.startTunnelCmd(), - Commands.waitUntil(() -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum))).withTimeout(IndexerK.kTunnelSpunUpTimeout), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), m_indexer.startSpindexerCmd() ) @@ -156,7 +162,6 @@ public void stopShooting() { m_indexer.stopSpindexer(); m_indexer.stopTunnel(); m_shooter.setShooterVelocity(ShooterK.kShooterZeroRPS); - } //---COPE COMMANDS @@ -183,6 +188,14 @@ public Command emergencyBarfCmd() { ); } + public Command lockTurret() { + return m_shooter.m_turret.setTurretLockCmd(true); + } + + public Command unlockTurret() { + return m_shooter.m_turret.setTurretLockCmd(false); + } + /** * @param isShooting if the shooter is currently shooting * @return a Command that unjams the shooter by running the spindexer, tunnel, and shooter (if it is running) backwards From d58c088f723118e67b599899b5bbab594880b73d Mon Sep 17 00:00:00 2001 From: Saarth Date: Wed, 25 Mar 2026 21:41:50 -0400 Subject: [PATCH 5/7] literally one condition lol still keeping the suppliers in case we want them --- .../java/frc/robot/subsystems/Intake.java | 4 ++ .../frc/robot/subsystems/Superstructure.java | 69 +++++++++++-------- 2 files changed, 45 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index cb4c02c8..1acb5647 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -167,6 +167,10 @@ public Command stopIntakeRollers() { return setIntakeRollersVelocityCmd(0); } + public Command barfIntakeRollers() { + return barfIntakeRollers(); + } + public void setIntakeRollersVelocity(double volts) { m_intakeRollers.setControl(m_VVReq.withOutput(volts)); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index c9c02a51..2dadc3aa 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -20,6 +20,7 @@ public class Superstructure extends SubsystemBase { private final Shooter m_shooter; /*TRUTH MACHINE SUPPLIERS*/ + // these aren't being used but i'm keebing them bc i spent too much time on them and they may be useful in the future private BooleanSupplier supp_intakeArmDeployed; private BooleanSupplier supp_intakeArmSafe; private BooleanSupplier supp_intakeArmShimmying; @@ -34,6 +35,8 @@ public class Superstructure extends SubsystemBase { private BooleanSupplier supp_isBarfing; private BooleanSupplier supp_isUnjamming; + private BooleanSupplier supp_canShoot; + /* LOGGERS */ @@ -56,6 +59,8 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { supp_isBarfing = () -> emergencyBarfCmd().isScheduled(); supp_isUnjamming = () -> unjamCmd(supp_isShooting).isScheduled(); + + supp_canShoot = () -> !supp_isBarfing.getAsBoolean(); } /* SUBSYSTEM COMMANDS */ @@ -111,48 +116,56 @@ public Command intakeArmShimmyCmd() { * @return a Command that starts and controls the overall shooting sequence logic */ public Command shootCmd(Supplier RPS) { - return Commands.parallel( - m_shooter.setShooterVelocityCmdSupp(RPS), + if (supp_canShoot.getAsBoolean()) { + return Commands.parallel( + m_shooter.setShooterVelocityCmdSupp(RPS), - Commands.sequence( - Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), - m_indexer.startTunnelCmd(), - Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), + Commands.sequence( + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), + m_indexer.startTunnelCmd(), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), - m_indexer.startSpindexerCmd(), + m_indexer.startSpindexerCmd(), - Commands.repeatingSequence( - Commands.none() + Commands.repeatingSequence( + Commands.none() + ) ) ) - ) - .finallyDo( - () -> { - stopShooting(); - } - ); + .finallyDo( + () -> { + stopShooting(); + } + ); + } else { + return Commands.none(); + } } /** * @return a Command that starts and controls the overall shooting sequence logic using the shotClauclator value */ public Command shootWithShotCalcCmd() { - return Commands.parallel( - m_shooter.shootFromCalc(), + if (supp_canShoot.getAsBoolean()) { + return Commands.parallel( + m_shooter.shootFromCalc(), - Commands.sequence( - Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), - m_indexer.startTunnelCmd(), - Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), + Commands.sequence( + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), + m_indexer.startTunnelCmd(), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), - m_indexer.startSpindexerCmd() + m_indexer.startSpindexerCmd() + ) ) - ) - .finallyDo( - () -> { - stopShooting(); - } - ); + .finallyDo( + () -> { + stopShooting(); + } + ); + } else { + return Commands.none(); + } } /** From ae621fa69989a2fd3401dfe505f304f8b00f0cfd Mon Sep 17 00:00:00 2001 From: Saarth Date: Wed, 25 Mar 2026 23:22:53 -0400 Subject: [PATCH 6/7] nvm, there was a second condition also fixed some naming inconsistency --- .../frc/robot/subsystems/Superstructure.java | 62 ++++++++++--------- 1 file changed, 34 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 2dadc3aa..68ed54f4 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -29,12 +29,13 @@ public class Superstructure extends SubsystemBase { private BooleanSupplier supp_shooterReadyToShoot; private BooleanSupplier supp_tunnelReadyToShoot; - private BooleanSupplier supp_turretIsLocked; - private BooleanSupplier supp_isShooting; + private BooleanSupplier supp_turretLocked; + private BooleanSupplier supp_shooting; - private BooleanSupplier supp_isBarfing; - private BooleanSupplier supp_isUnjamming; + private BooleanSupplier supp_barfing; + private BooleanSupplier supp_unjamming; + private BooleanSupplier supp_canIntake; private BooleanSupplier supp_canShoot; /* LOGGERS */ @@ -54,13 +55,14 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { supp_shooterReadyToShoot = () -> (m_shooter.isShooterSpunUp() && (m_shooter.getShooterVelocity().gte(ShooterK.kShooterSpunUpMinimum))); supp_tunnelReadyToShoot = () -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum)); - supp_turretIsLocked = () -> m_shooter.m_turret.getTurretLocked(); - supp_isShooting = () -> supp_shooterReadyToShoot.getAsBoolean() && supp_tunnelReadyToShoot.getAsBoolean(); + supp_turretLocked = () -> m_shooter.m_turret.getTurretLocked(); + supp_shooting = () -> supp_shooterReadyToShoot.getAsBoolean() && supp_tunnelReadyToShoot.getAsBoolean(); - supp_isBarfing = () -> emergencyBarfCmd().isScheduled(); - supp_isUnjamming = () -> unjamCmd(supp_isShooting).isScheduled(); + supp_barfing = () -> emergencyBarfCmd().isScheduled(); + supp_unjamming = () -> unjamCmd(supp_shooting).isScheduled(); - supp_canShoot = () -> !supp_isBarfing.getAsBoolean(); + supp_canIntake = () -> !supp_barfing.getAsBoolean(); + supp_canShoot = () -> !supp_barfing.getAsBoolean(); } /* SUBSYSTEM COMMANDS */ @@ -72,27 +74,31 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { * @return a Command that deploys the intake arm and runs the overall intaking logic */ public Command intakeCmd(BooleanSupplier isShooting) { - return Commands.sequence( - m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), - Commands.waitUntil(() -> m_intake.intakeArmAtTargetPos(0.01)).withTimeout(0.25), - Commands.run( - () -> { - boolean shooting = isShooting.getAsBoolean(); + if (supp_canIntake.getAsBoolean()) { + return Commands.sequence( + m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), + Commands.waitUntil(() -> m_intake.intakeArmAtTargetPos(0.01)).withTimeout(0.25), + Commands.run( + () -> { + boolean shooting = isShooting.getAsBoolean(); - m_shooter.m_turret.setIntaking(!shooting); - m_intake.setIntakeRollersVelocity(12); - m_indexer.setSpindexerVelocity(shooting ? IndexerK.kSpindexerShootRPS : IndexerK.kSpindexerIntakeRPS); - }) - ).finallyDo( - () -> { - boolean shooting = isShooting.getAsBoolean(); - m_shooter.m_turret.setIntaking(false); - m_intake.setIntakeRollersVelocity(0); - if (!shooting) { - m_indexer.setSpindexerVelocity(RotationsPerSecond.zero()); + m_shooter.m_turret.setIntaking(!shooting); + m_intake.setIntakeRollersVelocity(12); + m_indexer.setSpindexerVelocity(shooting ? IndexerK.kSpindexerShootRPS : IndexerK.kSpindexerIntakeRPS); + }) + ).finallyDo( + () -> { + boolean shooting = isShooting.getAsBoolean(); + m_shooter.m_turret.setIntaking(false); + m_intake.setIntakeRollersVelocity(0); + if (!shooting) { + m_indexer.setSpindexerVelocity(RotationsPerSecond.zero()); + } } - } - ); + ); + } else { + return Commands.none(); + } } /** From 0f47ba49331cb1b9f32b152cb4d4a6b554dd7bea Mon Sep 17 00:00:00 2001 From: Saarth Date: Sat, 28 Mar 2026 01:21:51 -0400 Subject: [PATCH 7/7] fixed the massive .isScheduled() cope and actually made things work (copefully) robot code is like an onion --- src/main/java/frc/robot/Robot.java | 17 ++- .../frc/robot/subsystems/Superstructure.java | 115 ++++++++---------- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- 3 files changed, 56 insertions(+), 78 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 650de07b..a94bffc0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -111,9 +111,6 @@ public class Robot extends TimedRobot { private Trigger trg_manipOverride = m_manipulator.b(); //---COMMAND SEQUENCE TRIGGERS - - // private Trigger trg_activateIntake = m_manipulator.a().and(trg_manipOverride.negate()); - // private Trigger trg_safeIntake = m_manipulator.x().and(trg_manipOverride.negate()); private Trigger trg_intake = m_manipulator.rightTrigger().and(trg_manipOverride.negate()); private Trigger trg_retractIntake = m_manipulator.rightBumper().and(trg_manipOverride.negate()); @@ -131,6 +128,10 @@ public class Robot extends TimedRobot { private Trigger trg_unjam = m_driver.rightBumper(); + //---SAFETY TRIGGERS + private Trigger trg_canIntake = trg_emergencyBarf.negate(); + private Trigger trg_canShoot = trg_emergencyBarf.negate(); + /* LOGGERS */ private final DoubleLogger log_stickDesiredFieldX = WaltLogger.logDouble("Swerve", "stick desired teleop x"); @@ -264,8 +265,8 @@ private void configureBindings() { //---NORMAL SEQUENCES //Intake - trg_intake.and(trg_shoot.negate()).whileTrue( - m_superstructure.intakeCmd(() -> false) + trg_intake.and(trg_canIntake).whileTrue( + m_superstructure.intakeCmd(() -> trg_shoot.getAsBoolean()) ); trg_retractIntake.onTrue( @@ -275,7 +276,7 @@ private void configureBindings() { //Shooting // NORMAL FIXED SHOT // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - trg_shoot.whileTrue(m_superstructure.shootWithShotCalcCmd()); //comment out for LERP with above + trg_shoot.and(trg_canShoot).whileTrue(m_superstructure.shootWithShotCalcCmd()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); @@ -290,10 +291,6 @@ private void configureBindings() { // snapshot on each shoot press trg_shoot.onTrue(WaltCamera.takeSnapshotCmd()); - trg_intake.and(trg_shoot).whileTrue( - m_superstructure.intakeCmd(() -> true) - ); - trg_emergencyBarf.whileTrue( m_superstructure.emergencyBarfCmd() ); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 68ed54f4..67fc3d0d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IndexerK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; @@ -35,9 +36,6 @@ public class Superstructure extends SubsystemBase { private BooleanSupplier supp_barfing; private BooleanSupplier supp_unjamming; - private BooleanSupplier supp_canIntake; - private BooleanSupplier supp_canShoot; - /* LOGGERS */ @@ -60,9 +58,6 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { supp_barfing = () -> emergencyBarfCmd().isScheduled(); supp_unjamming = () -> unjamCmd(supp_shooting).isScheduled(); - - supp_canIntake = () -> !supp_barfing.getAsBoolean(); - supp_canShoot = () -> !supp_barfing.getAsBoolean(); } /* SUBSYSTEM COMMANDS */ @@ -74,31 +69,27 @@ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { * @return a Command that deploys the intake arm and runs the overall intaking logic */ public Command intakeCmd(BooleanSupplier isShooting) { - if (supp_canIntake.getAsBoolean()) { - return Commands.sequence( - m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), - Commands.waitUntil(() -> m_intake.intakeArmAtTargetPos(0.01)).withTimeout(0.25), - Commands.run( - () -> { - boolean shooting = isShooting.getAsBoolean(); - - m_shooter.m_turret.setIntaking(!shooting); - m_intake.setIntakeRollersVelocity(12); - m_indexer.setSpindexerVelocity(shooting ? IndexerK.kSpindexerShootRPS : IndexerK.kSpindexerIntakeRPS); - }) - ).finallyDo( - () -> { - boolean shooting = isShooting.getAsBoolean(); - m_shooter.m_turret.setIntaking(false); - m_intake.setIntakeRollersVelocity(0); - if (!shooting) { - m_indexer.setSpindexerVelocity(RotationsPerSecond.zero()); - } + return Commands.sequence( + m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), + Commands.waitUntil(() -> m_intake.intakeArmAtTargetPos(0.01)).withTimeout(0.25), + Commands.run( + () -> { + boolean shooting = isShooting.getAsBoolean(); + + m_shooter.m_turret.setIntaking(!shooting); + m_intake.setIntakeRollersVelocity(12); + m_indexer.setSpindexerVelocity(shooting ? IndexerK.kSpindexerShootRPS : IndexerK.kSpindexerIntakeRPS); + }) + ).finallyDo( + () -> { + boolean shooting = isShooting.getAsBoolean(); + m_shooter.m_turret.setIntaking(false); + m_intake.setIntakeRollersVelocity(0); + if (!shooting) { + m_indexer.setSpindexerVelocity(RotationsPerSecond.zero()); } - ); - } else { - return Commands.none(); - } + } + ); } /** @@ -122,56 +113,46 @@ public Command intakeArmShimmyCmd() { * @return a Command that starts and controls the overall shooting sequence logic */ public Command shootCmd(Supplier RPS) { - if (supp_canShoot.getAsBoolean()) { - return Commands.parallel( - m_shooter.setShooterVelocityCmdSupp(RPS), + return Commands.parallel( + m_shooter.setShooterVelocityCmdSupp(RPS), - Commands.sequence( - Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), - m_indexer.startTunnelCmd(), - Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), + Commands.sequence( + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), + m_indexer.startTunnelCmd(), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), - m_indexer.startSpindexerCmd(), + m_indexer.startSpindexerCmd(), - Commands.repeatingSequence( - Commands.none() - ) - ) + Commands.none().repeatedly() ) - .finallyDo( - () -> { - stopShooting(); - } - ); - } else { - return Commands.none(); - } + ) + .finallyDo( + () -> { + stopShooting(); + } + ); } /** * @return a Command that starts and controls the overall shooting sequence logic using the shotClauclator value */ public Command shootWithShotCalcCmd() { - if (supp_canShoot.getAsBoolean()) { - return Commands.parallel( - m_shooter.shootFromCalc(), + return Commands.parallel( + m_shooter.shootFromCalc(), - Commands.sequence( - Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), - m_indexer.startTunnelCmd(), - Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), + Commands.sequence( + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), + m_indexer.startTunnelCmd(), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), - m_indexer.startSpindexerCmd() - ) + m_indexer.startSpindexerCmd() ) - .finallyDo( - () -> { - stopShooting(); - } - ); - } else { - return Commands.none(); - } + ) + .finallyDo( + () -> { + stopShooting(); + } + ); } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c008187e..91c3aec2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -167,7 +167,7 @@ public Command setShooterVelocityCmd(AngularVelocity RPS) { } public Command setShooterVelocityCmdSupp(Supplier supp_RPS) { - return runOnce(() -> setShooterVelocity(supp_RPS.get())); + return run(() -> setShooterVelocity(supp_RPS.get())); } public void setShooterVelocitySupp(Supplier supp_RPS) {