diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 15811f93..87aad4fa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -229,6 +229,7 @@ public void robotPeriodic() { superPoopParameters.flywheelSpeeds().rightSpeed()); // Robot container periodic methods + robotContainer.updateDemoControls(); robotContainer.checkControllers(); robotContainer.updateDashboardOutputs(); robotContainer.updateAlerts(); diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 78f21948..4831b8e0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -20,6 +20,8 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -79,6 +81,7 @@ import org.littletonrobotics.frc2024.util.Alert.AlertType; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** @@ -123,6 +126,8 @@ public class RobotContainer { new Alert("Operator controller disconnected (port 1).", AlertType.WARNING); private final Alert overrideDisconnected = new Alert("Override controller disconnected (port 5).", AlertType.INFO); + private final Alert demoControlsActivated = + new Alert("Demo controls are active. Do not use in competition.", AlertType.INFO); private final LoggedDashboardNumber endgameAlert1 = new LoggedDashboardNumber("Endgame Alert #1", 30.0); private final LoggedDashboardNumber endgameAlert2 = @@ -130,9 +135,16 @@ public class RobotContainer { private boolean podiumShotMode = false; private boolean armCoastOverride = false; + private boolean lastWasDemoControls = false; // Dashboard inputs private final AutoSelector autoSelector = new AutoSelector("Auto"); + private final LoggedDashboardChooser demoControls = + new LoggedDashboardChooser<>("Demo Mode"); + private final LoggedDashboardChooser demoSpeedChooser = + new LoggedDashboardChooser<>("Demo Speed"); + private final LoggedDashboardChooser demoShotChooser = + new LoggedDashboardChooser<>("Demo Shot"); /** Returns the current AprilTag layout type. */ public AprilTagLayoutType getAprilTagLayoutType() { @@ -299,22 +311,23 @@ public RobotContainer() { .onFalse(Commands.runOnce(() -> armCoastOverride = false).ignoringDisable(true)); RobotModeTriggers.disabled() .onFalse(Commands.runOnce(() -> armCoastOverride = false).ignoringDisable(true)); - arm.setOverrides(armDisable, () -> armCoastOverride); + arm.setOverrides(armDisable, () -> armCoastOverride, demoControls::get); climber.setCoastOverride(() -> armCoastOverride); backpackActuator.setCoastOverride(() -> armCoastOverride); robotState.setLookaheadDisable(lookaheadDisable); flywheels.setPrepareShootSupplier( () -> - autoFlywheelSpinupDisable.negate().getAsBoolean() + !autoFlywheelSpinupDisable.getAsBoolean() && DriverStation.isTeleopEnabled() && !superstructure.getCurrentGoal().isClimbingGoal() && (robotState.inCloseShootingZone() || (robotState.inShootingZone() - && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED))); + && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED)) + && !demoControls.get()); // Configure autos and buttons configureAutos(); - configureButtonBindings(); + configureButtonBindings(false); // Alerts for constants if (Constants.tuningMode) { @@ -435,6 +448,34 @@ private void configureAutos() { .withName("Drive Wheel Radius Characterization")); autoSelector.addRoutine( "Diagnose Arm", superstructure.setGoalCommand(Superstructure.Goal.DIAGNOSTIC_ARM)); + + // Add options to demo mode + demoControls.addOption("YES", true); + demoControls.addDefaultOption("NO", false); + + // Set speed scalar chooser for demo + demoSpeedChooser.addDefaultOption("Full (100%)", 1.0); + demoSpeedChooser.addOption("Fast (70%)", 0.7); + demoSpeedChooser.addOption("Medium (30%)", 0.3); + demoSpeedChooser.addOption("Slow (15%)", 0.15); + + // Set shot modes for demo shot + demoShotChooser.addDefaultOption( + "Short", + new RobotState.DemoShotParameters( + Rotation2d.fromDegrees(35.0), new RobotState.FlywheelSpeeds(1200, 1200))); + demoShotChooser.addOption( + "Medium", + new RobotState.DemoShotParameters( + Rotation2d.fromDegrees(35.0), new RobotState.FlywheelSpeeds(3500, 4000))); + demoShotChooser.addOption( + "Long", + new RobotState.DemoShotParameters( + Rotation2d.fromDegrees(40.0), new RobotState.FlywheelSpeeds(5000, 8000))); + demoShotChooser.addOption( + "Tall", + new RobotState.DemoShotParameters( + Rotation2d.fromDegrees(80.0), new RobotState.FlywheelSpeeds(5000, 8000))); } /** @@ -442,7 +483,9 @@ private void configureAutos() { * instantiating a {@link GenericHID} or one of its subclasses ({@link Joystick} or {@link * XboxController}), and then passing it to a {@link JoystickButton}. */ - private void configureButtonBindings() { + private void configureButtonBindings(boolean demo) { + CommandScheduler.getInstance().getActiveButtonLoop().clear(); + // ------------- Driver Controls ------------- drive.setDefaultCommand( drive @@ -456,95 +499,123 @@ private void configureButtonBindings() { .withName("Drive Teleop Input")); // ------------- Shooting Controls ------------- - // Aim and rev flywheels - Supplier superstructureAimCommand = - () -> - Commands.either( - Commands.either( - superstructure.setGoalCommand(Superstructure.Goal.PODIUM), - superstructure.setGoalCommand(Superstructure.Goal.SUBWOOFER), - () -> podiumShotMode), - superstructure.setGoalCommand(Superstructure.Goal.AIM), - shootPresets); - Supplier driveAimCommand = - () -> - Commands.either( - Commands.none(), - Commands.startEnd( - () -> - drive.setHeadingGoal(() -> robotState.getAimingParameters().driveHeading()), - drive::clearHeadingGoal), - shootAlignDisable); - Trigger nearSpeaker = new Trigger(robotState::inShootingZone); Trigger intakeTrigger = driver.leftTrigger(); - driver - .a() - .and(intakeTrigger.negate()) - .and(nearSpeaker.or(shootPresets)) - .whileTrueContinuous( - driveAimCommand - .get() - .alongWith(superstructureAimCommand.get(), flywheels.shootCommand()) - .withName("Prepare Shot")); - driver - .a() - .and(intakeTrigger.negate()) - .and(nearSpeaker.negate().and(shootPresets.negate())) - .whileTrueContinuous( - Commands.startEnd( - () -> - drive.setHeadingGoal( - () -> robotState.getSuperPoopAimingParameters().driveHeading()), - drive::clearHeadingGoal) - .alongWith( - superstructure.setGoalCommand(Superstructure.Goal.SUPER_POOP), - flywheels.superPoopCommand()) - .withName("Super Poop")); readyToShoot = new Trigger( () -> drive.atHeadingGoal() && superstructure.atArmGoal() && flywheels.atGoal()); - driver - .rightTrigger() - .and(driver.a()) - .and(nearSpeaker.or(shootPresets)) - .and(readyToShoot.debounce(0.2, DebounceType.kRising)) - .onTrue( - Commands.parallel( - Commands.waitSeconds(0.5), Commands.waitUntil(driver.rightTrigger().negate())) - .deadlineWith( - rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER), - superstructureAimCommand.get(), - flywheels.shootCommand(), - driveAimCommand.get()) - .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)); - driver - .rightTrigger() - .and(driver.a()) - .and(nearSpeaker.negate().and(shootPresets.negate())) - .and(readyToShoot.debounce(0.3, DebounceType.kFalling)) - .onTrue( - Commands.parallel( - Commands.waitSeconds(0.5), Commands.waitUntil(driver.rightTrigger().negate())) - .deadlineWith( - rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER), - superstructure.setGoalCommand(Superstructure.Goal.SUPER_POOP), - flywheels.superPoopCommand(), - driveAimCommand.get()) - .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)); - - driver.a().and(readyToShoot).whileTrue(controllerRumbleCommand()); - - // Poop. - driver - .rightTrigger() - .and(driver.a().negate()) - .and(driver.b().negate()) - .whileTrue( - flywheels - .poopCommand() - .alongWith( - Commands.waitUntil(flywheels::atGoal) - .andThen(rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER)))); + if (!demo) { + // Aim and rev flywheels + Supplier superstructureAimCommand = + () -> + Commands.either( + Commands.either( + superstructure.setGoalCommand(Superstructure.Goal.PODIUM), + superstructure.setGoalCommand(Superstructure.Goal.SUBWOOFER), + () -> podiumShotMode), + superstructure.setGoalCommand(Superstructure.Goal.AIM), + shootPresets); + Supplier driveAimCommand = + () -> + Commands.either( + Commands.none(), + Commands.startEnd( + () -> + drive.setHeadingGoal( + () -> robotState.getAimingParameters().driveHeading()), + drive::clearHeadingGoal), + shootAlignDisable); + Trigger nearSpeaker = new Trigger(robotState::inShootingZone); + driver + .a() + .and(intakeTrigger.negate()) + .and(nearSpeaker.or(shootPresets)) + .whileTrueContinuous( + driveAimCommand + .get() + .alongWith(superstructureAimCommand.get(), flywheels.shootCommand()) + .withName("Prepare Shot")); + driver + .a() + .and(intakeTrigger.negate()) + .and(nearSpeaker.negate().and(shootPresets.negate())) + .whileTrueContinuous( + Commands.startEnd( + () -> + drive.setHeadingGoal( + () -> robotState.getSuperPoopAimingParameters().driveHeading()), + drive::clearHeadingGoal) + .alongWith( + superstructure.setGoalCommand(Superstructure.Goal.SUPER_POOP), + flywheels.superPoopCommand()) + .withName("Super Poop")); + driver + .rightTrigger() + .and(driver.a()) + .and(nearSpeaker.or(shootPresets)) + .and(readyToShoot.debounce(0.2, DebounceType.kRising)) + .onTrue( + Commands.parallel( + Commands.waitSeconds(0.5), Commands.waitUntil(driver.rightTrigger().negate())) + .deadlineWith( + rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER), + superstructureAimCommand.get(), + flywheels.shootCommand(), + driveAimCommand.get()) + .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)); + driver + .rightTrigger() + .and(driver.a()) + .and(nearSpeaker.negate().and(shootPresets.negate())) + .and(readyToShoot.debounce(0.3, DebounceType.kFalling)) + .onTrue( + Commands.parallel( + Commands.waitSeconds(0.5), Commands.waitUntil(driver.rightTrigger().negate())) + .deadlineWith( + rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER), + superstructure.setGoalCommand(Superstructure.Goal.SUPER_POOP), + flywheels.superPoopCommand(), + driveAimCommand.get()) + .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)); + + driver.a().and(readyToShoot).whileTrue(controllerRumbleCommand()); + + // Poop. + driver + .rightTrigger() + .and(driver.a().negate()) + .and(driver.b().negate()) + .whileTrue( + flywheels + .poopCommand() + .alongWith( + Commands.waitUntil(flywheels::atGoal) + .andThen(rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER)))); + } else { + new Trigger(DriverStation::isEnabled) + .whileTrueContinuous( + Commands.run( + () -> RobotState.getInstance().setDemoShotParameters(demoShotChooser.get()))); + driver + .a() + .and(intakeTrigger.negate()) + .whileTrueContinuous( + superstructure + .setGoalWithConstraintsCommand( + Superstructure.Goal.DEMO_SHOT, Arm.smoothProfileConstraints.get()) + .alongWith(flywheels.demoShootCommand())); + driver + .a() + .and(readyToShoot.debounce(0.8, DebounceType.kRising)) + .onTrue( + rollers + .setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER) + .alongWith( + superstructure.setGoalWithConstraintsCommand( + Superstructure.Goal.DEMO_SHOT, Arm.smoothProfileConstraints.get()), + flywheels.demoShootCommand()) + .withTimeout(0.6) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + } // ------------- Intake Controls ------------- // Intake Floor @@ -570,14 +641,28 @@ private void configureButtonBindings() { .whileTrue(rollers.setGoalCommand(Rollers.Goal.EJECT_TO_FLOOR).withName("Eject To Floor")); // Intake source - driver - .rightBumper() - .whileTrue( - superstructure - .setGoalCommand(Superstructure.Goal.STATION_INTAKE) - .alongWith( - rollers.setGoalCommand(Rollers.Goal.STATION_INTAKE), flywheels.intakeCommand()) - .withName("Source Intake")); + if (!demo) { + driver + .rightBumper() + .whileTrue( + superstructure + .setGoalCommand(Superstructure.Goal.STATION_INTAKE) + .alongWith( + rollers.setGoalCommand(Rollers.Goal.STATION_INTAKE), + flywheels.intakeCommand()) + .withName("Source Intake")); + } else { + driver + .rightBumper() + .whileTrue( + superstructure + .setGoalWithConstraintsCommand( + Superstructure.Goal.STOW, Arm.smoothProfileConstraints.get()) + .alongWith( + rollers.setGoalCommand(Rollers.Goal.DEMO_STATION_INTAKE), + flywheels.demoIntakeCommand()) + .withName("Demo Source Intake")); + } // ------------- Amp Scoring Controls ------------- Container ampAlignedDriverTranslation = new Container<>(); @@ -653,18 +738,20 @@ private void configureButtonBindings() { ampAutoDrive, // Drive while heading is being controlled - drive - .run( - () -> - drive.acceptTeleopInput( - -driver.getLeftY(), - -driver.getLeftX(), - 0.0, - robotRelative.getAsBoolean())) - .alongWith( - Commands.startEnd( - () -> drive.setHeadingGoal(() -> Rotation2d.fromDegrees(-90.0)), - drive::clearHeadingGoal)), + demo + ? Commands.none() + : drive + .run( + () -> + drive.acceptTeleopInput( + -driver.getLeftY(), + -driver.getLeftX(), + 0.0, + robotRelative.getAsBoolean())) + .alongWith( + Commands.startEnd( + () -> drive.setHeadingGoal(() -> Rotation2d.fromDegrees(-90.0)), + drive::clearHeadingGoal)), autoDriveEnable) .alongWith( Commands.waitUntil( @@ -790,6 +877,22 @@ private Command controllerRumbleCommand() { }); } + /** Update demo controls. */ + public void updateDemoControls() { + // Update control binding + if (demoControls.get() != lastWasDemoControls) { + configureButtonBindings(demoControls.get()); + lastWasDemoControls = demoControls.get(); + } + + // Update teleop speed + TeleopDriveController.setVelocityScalar(demoControls.get() ? demoSpeedChooser.get() : 1.0); + + // Update alerts + Leds.getInstance().demoMode = demoControls.get(); + demoControlsActivated.set(demoControls.get()); + } + /** Updates the alerts for disconnected controllers. */ public void checkControllers() { driverDisconnected.set( diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 2c28467f..b6dc746c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -52,6 +52,8 @@ public record AimingParameters( double effectiveDistance, FlywheelSpeeds flywheelSpeeds) {} + public record DemoShotParameters(Rotation2d armAngle, FlywheelSpeeds flywheelSpeeds) {} + private static final LoggedTunableNumber autoLookahead = new LoggedTunableNumber("RobotState/AutoLookahead", 0.5); private static final LoggedTunableNumber lookahead = @@ -129,6 +131,10 @@ public static RobotState getInstance() { private AimingParameters latestSuperPoopParameters = null; + @Setter @Getter + private DemoShotParameters demoShotParameters = + new DemoShotParameters(Rotation2d.fromDegrees(0.0), new FlywheelSpeeds(0.0, 0.0)); + @Setter private BooleanSupplier lookaheadDisable = () -> false; private RobotState() { diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java index 31e9c9b1..93b651e8 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; +import lombok.Setter; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @@ -26,6 +27,8 @@ public class TeleopDriveController { private static final LoggedTunableNumber maxAngularVelocityScalar = new LoggedTunableNumber("TeleopDrive/MaxAngularVelocityScalar", 0.65); + @Setter private static double velocityScalar = 1.0; + private double controllerX = 0; private double controllerY = 0; private double controllerOmega = 0; @@ -56,12 +59,13 @@ public ChassisSpeeds update() { double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get()); omega = Math.copySign(omega * omega, omega); + final double maxLinearVelocity = driveConfig.maxLinearVelocity() * velocityScalar; final double maxAngularVelocity = - driveConfig.maxAngularVelocity() * maxAngularVelocityScalar.get(); + driveConfig.maxAngularVelocity() * (maxAngularVelocityScalar.get() * velocityScalar); if (robotRelative) { return new ChassisSpeeds( - linearVelocity.getX() * driveConfig.maxLinearVelocity(), - linearVelocity.getY() * driveConfig.maxLinearVelocity(), + linearVelocity.getX() * maxLinearVelocity, + linearVelocity.getY() * maxLinearVelocity, omega * maxAngularVelocity); } else { if (DriverStation.getAlliance().isPresent() @@ -69,8 +73,8 @@ public ChassisSpeeds update() { linearVelocity = linearVelocity.rotateBy(Rotation2d.fromRadians(Math.PI)); } return ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * driveConfig.maxLinearVelocity(), - linearVelocity.getY() * driveConfig.maxLinearVelocity(), + linearVelocity.getX() * maxLinearVelocity, + linearVelocity.getY() * maxLinearVelocity, omega * maxAngularVelocity, RobotState.getInstance().getEstimatedPose().getRotation()); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 0a876af2..d1979da2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -41,6 +41,8 @@ public class Flywheels extends SubsystemBase { new LoggedTunableNumber("Flywheels/PrepareShootMultiplier", 1.0); private static final LoggedTunableNumber intakingRpm = new LoggedTunableNumber("Flywheels/IntakingRpm", -3000.0); + private static final LoggedTunableNumber demoIntakingRpm = + new LoggedTunableNumber("Flywheels/DemoIntakingRpm", -250.0); private static final LoggedTunableNumber ejectingRpm = new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0); private static final LoggedTunableNumber poopingRpm = @@ -70,12 +72,16 @@ public enum Goal { IDLE(() -> 0.0, () -> 0.0), SHOOT(shootingLeftRpm, shootingRightRpm), INTAKE(intakingRpm, intakingRpm), + DEMO_INTAKE(demoIntakingRpm, demoIntakingRpm), EJECT(ejectingRpm, ejectingRpm), POOP(poopingRpm, poopingRpm), SUPER_POOP( () -> RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().leftSpeed(), () -> RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().rightSpeed()), + DEMO_SHOT( + () -> RobotState.getInstance().getDemoShotParameters().flywheelSpeeds().leftSpeed(), + () -> RobotState.getInstance().getDemoShotParameters().flywheelSpeeds().rightSpeed()), CHARACTERIZING(() -> 0.0, () -> 0.0); private final DoubleSupplier leftGoal; @@ -226,6 +232,11 @@ public Command intakeCommand() { .withName("Flywheels Intake"); } + public Command demoIntakeCommand() { + return startEnd(() -> setGoal(Goal.DEMO_INTAKE), () -> setGoal(Goal.IDLE)) + .withName("Flywheels Demo Intake"); + } + public Command ejectCommand() { return startEnd(() -> setGoal(Goal.EJECT), () -> setGoal(Goal.IDLE)) .withName("Flywheels Eject"); @@ -239,4 +250,9 @@ public Command superPoopCommand() { return startEnd(() -> setGoal(Goal.SUPER_POOP), () -> setGoal(Goal.IDLE)) .withName("Flywheels Super Poop"); } + + public Command demoShootCommand() { + return startEnd(() -> setGoal(Goal.DEMO_SHOT), () -> setGoal(Goal.IDLE)) + .withName("Flywheels Demo Shoot"); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java index 452a9238..9eb680e3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java @@ -180,6 +180,8 @@ public synchronized void periodic() { rainbow(rainbowCycleLength, rainbowDuration); } else if (hasNote) { solid(Color.kGreen); + } else if (demoMode) { + wave(allianceColor, secondaryDisabledColor, waveAllianceCycleLength, waveAllianceDuration); } if (endgameAlert) { diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java index 71946ea5..83c81843 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -30,6 +30,10 @@ public class Rollers extends SubsystemBase { private static final LoggedTunableNumber jackhammerTime = new LoggedTunableNumber("Rollers/JackhammerTime", 0.075); + private static final LoggedTunableNumber stationIntakeTime = + new LoggedTunableNumber("Rollers/StationIntakeTime", 0.06); + private static final LoggedTunableNumber demoStationIntakeTime = + new LoggedTunableNumber("Rollers/DemoStationIntakeTime", 0.1); private final Feeder feeder; private final Indexer indexer; @@ -44,6 +48,7 @@ public enum Goal { IDLE, FLOOR_INTAKE, STATION_INTAKE, + DEMO_STATION_INTAKE, EJECT_TO_FLOOR, UNJAM_UNTACO, UNJAM_FEEDER, @@ -131,7 +136,16 @@ public void periodic() { } } case STATION_INTAKE -> { - if (gamepieceState != GamepieceState.NONE && gamepieceStateTimer.hasElapsed(0.06)) { + if (gamepieceState != GamepieceState.NONE + && gamepieceStateTimer.hasElapsed(stationIntakeTime.get())) { + indexer.setGoal(Indexer.Goal.IDLING); + } else { + indexer.setGoal(Indexer.Goal.STATION_INTAKING); + } + } + case DEMO_STATION_INTAKE -> { + if (gamepieceState != GamepieceState.NONE + && gamepieceStateTimer.hasElapsed(demoStationIntakeTime.get())) { indexer.setGoal(Indexer.Goal.IDLING); } else { indexer.setGoal(Indexer.Goal.STATION_INTAKING); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index 26429ca9..1ac593f1 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -31,6 +31,7 @@ public enum Goal { BACKPACK_OUT_UNJAM, AIM, SUPER_POOP, + DEMO_SHOT, UNJAM_FEEDER, STATION_INTAKE, AMP, @@ -116,6 +117,11 @@ public void periodic() { climber.setGoal(Climber.Goal.IDLE); backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } + case DEMO_SHOT -> { + arm.setGoal(Arm.Goal.DEMO_SHOT); + climber.setGoal(Climber.Goal.IDLE); + backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); + } case STATION_INTAKE -> { arm.setGoal(Arm.Goal.STATION_INTAKE); climber.setGoal(Climber.Goal.IDLE); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index fd0be936..039e5a9f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -81,6 +81,7 @@ public enum Goal { AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()), SUPER_POOP( () -> RobotState.getInstance().getSuperPoopAimingParameters().armAngle().getDegrees()), + DEMO_SHOT(() -> RobotState.getInstance().getDemoShotParameters().armAngle().getDegrees()), AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)), SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)), PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 34.0)), @@ -125,6 +126,7 @@ private double getRads() { private BooleanSupplier disableSupplier = DriverStation::isDisabled; private BooleanSupplier coastSupplier = () -> false; + private BooleanSupplier halfStowSupplier = () -> true; private boolean brakeModeEnabled = true; private boolean wasNotAuto = false; @@ -146,13 +148,19 @@ public Arm(ArmIO io) { goalVisualizer = new ArmVisualizer("Goal", Color.kBlue); } - public void setOverrides(BooleanSupplier disableOverride, BooleanSupplier coastOverride) { + public void setOverrides( + BooleanSupplier disableOverride, + BooleanSupplier coastOverride, + BooleanSupplier halfStowOverride) { disableSupplier = () -> disableOverride.getAsBoolean() || DriverStation.isDisabled(); coastSupplier = coastOverride; + halfStowSupplier = halfStowOverride; } private double getStowAngle() { - if (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()) { + if (DriverStation.isTeleopEnabled() + && RobotState.getInstance().inCloseShootingZone() + && !halfStowSupplier.getAsBoolean()) { return MathUtil.clamp( setpointState.position, minAngle.getRadians(),