diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 995f7f19..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.intake(() -> 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.activateOuttakeShotCalc()); //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,22 +291,14 @@ private void configureBindings() { // snapshot on each shoot press trg_shoot.onTrue(WaltCamera.takeSnapshotCmd()); - trg_intake.and(trg_shoot).whileTrue( - m_superstructure.intake(() -> true) - ); - trg_emergencyBarf.whileTrue( - m_superstructure.emergencyBarf() + m_superstructure.emergencyBarfCmd() ); - trg_shimmy.whileTrue(m_superstructure.shimmy()); + 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 @@ -315,18 +308,18 @@ 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 - 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/autons/WaltSimpleAutonFactory.java b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java index 254d64e4..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.activateOuttakeShotCalc(), //(() -> 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.activateOuttakeShotCalc() + 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.activateOuttakeShotCalc() + 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/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0a880d2b..1acb5647 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -121,21 +121,32 @@ 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 Command shimmy() { + 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() { + return Commands.sequence( + stopIntakeRollers(), + setIntakeArmPosCmd(IntakeArmPosition.RETRACTED) + ); + } + + public Command intakeArmShimmy() { return Commands.repeatingSequence( 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)); } @@ -156,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)); } @@ -164,6 +179,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 ae75f462..67fc3d0d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,26 +1,16 @@ - 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 edu.wpi.first.wpilibj2.command.button.Trigger; 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; @@ -30,33 +20,58 @@ public class Superstructure extends SubsystemBase { private final Indexer m_indexer; private final Shooter m_shooter; - /* LOGGERS */ - private HashSet m_activeCommands = new HashSet<>(); - private final StringArrayLogger log_activeCommands = WaltLogger.logStringArray(kLogTab, "Active Commands"); + /*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; + private BooleanSupplier supp_intaking; + + private BooleanSupplier supp_shooterReadyToShoot; + private BooleanSupplier supp_tunnelReadyToShoot; - private HashSet m_activeOverrideCommands = new HashSet<>(); - private final StringArrayLogger log_activeOverrideCommands = WaltLogger.logStringArray(kLogTab, "Active Override Commands"); + private BooleanSupplier supp_turretLocked; + private BooleanSupplier supp_shooting; + + private BooleanSupplier supp_barfing; + private BooleanSupplier supp_unjamming; + + /* LOGGERS */ - // private StringLogger log_shooterState = WaltLogger.logString(kLogTab, "ShooterState"); /* CONSTRUCTOR */ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { m_intake = intake; m_indexer = indexer; m_shooter = shooter; + + 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_turretLocked = () -> m_shooter.m_turret.getTurretLocked(); + supp_shooting = () -> supp_shooterReadyToShoot.getAsBoolean() && supp_tunnelReadyToShoot.getAsBoolean(); + + supp_barfing = () -> emergencyBarfCmd().isScheduled(); + supp_unjamming = () -> unjamCmd(supp_shooting).isScheduled(); } - /* BUTTON BIND SEQUENCES */ + /* SUBSYSTEM COMMANDS */ + + //---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), + Commands.waitUntil(() -> m_intake.intakeArmAtTargetPos(0.01)).withTimeout(0.25), Commands.run( () -> { boolean shooting = isShooting.getAsBoolean(); @@ -77,136 +92,87 @@ public Command intake(BooleanSupplier isShooting) { ); } - public Command startShootSequenceNOSHOOT() { - return Commands.parallel( - Commands.sequence( - m_indexer.startTunnelCmd(), - m_indexer.startSpindexerCmd() - ) - ); + /** + * @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(); } /** - * @param update message to update the SHOOTER logger - * @return a Command that updates the logger + * @return a Command that calls intakeArmShimmy (oscillates the intakeArm between the DEPLOYED and SHIMMY position) */ - // public Command up(String update) { - // return Commands.runOnce(() -> log_shooterState.accept(update)); - // } + 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 activateOuttake(Supplier RPS) { - // log_shooterState.accept("pre sequence"); + public Command shootCmd(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), + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), m_indexer.startTunnelCmd(), - Commands.waitUntil(() -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum))).withTimeout(IndexerK.kTunnelSpunUpTimeout), - // up("post waituntil command"), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), - // 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() - ) + + Commands.none().repeatedly() ) - // up("post repeating sequence") ) .finallyDo( () -> { - deactivateOuttake(); - // log_shooterState.accept("post deactivate outtake"); + stopShooting(); } ); } /** - * 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 activateOuttakeShotCalc() { - // log_shooterState.accept("pre sequence"); + public Command shootWithShotCalcCmd() { 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), + Commands.waitUntil(supp_shooterReadyToShoot).withTimeout(ShooterK.kShooterSpunUpTimeout), m_indexer.startTunnelCmd(), - Commands.waitUntil(() -> (m_indexer.isTunnelSpunUp()) && (m_indexer.getTunnelVelocity().gte(IndexerK.kTunnelSpunUpMinimum))).withTimeout(IndexerK.kTunnelSpunUpTimeout), - // up("post waituntil command"), + Commands.waitUntil(supp_tunnelReadyToShoot).withTimeout(IndexerK.kTunnelSpunUpTimeout), - // 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. + * Stops shooting by turning off the spindexer, tunnel, and shooter */ - 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)); + //---COPE COMMANDS - // Commands.sequence(logActiveCommands("deactivateOuttake", "shooting", "emergencyDump")); - } - - public Command emergencyBarf() { + /** + * @return a Command that ejects fuel from the intake and shooter while active + */ + public Command emergencyBarfCmd() { 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,65 +182,24 @@ 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(); + public Command lockTurret() { + return m_shooter.m_turret.setTurretLockCmd(true); } - /** - * 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 - ); + public Command unlockTurret() { + return m_shooter.m_turret.setTurretLockCmd(false); } /** - * Raises/lowers the hood to the given degs. - * @param degs degrees to raise/lower to. - * @return + * @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 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 +221,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 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) {