diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 972d283e..278ddba6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Inches; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.PowerDistribution; @@ -48,6 +50,7 @@ import frc.robot.subsystems.Telemetry; import frc.robot.subsystems.Turret; import frc.robot.subsystems.Turret.TurretConstants; +import frc.util.AllianceHelpers; import frc.util.leds.Color; import frc.util.leds.LEDBehaviorFactory; import frc.util.leds.LEDBooleanSupplier; @@ -72,7 +75,7 @@ public class RobotContainer { private final PhotonVision vision; private final Telemetry logger; private final Cannon cannon; - + private SendableChooser autoChooser = new SendableChooser<>(); public RobotContainer() { @@ -91,8 +94,7 @@ public RobotContainer() { hood = new Hood(); turret = new Turret(drivetrain); cannon = new Cannon(shooter, turret, hood, drivetrain, indexer); - vision = new PhotonVision(drivetrain); - + vision = new PhotonVision(drivetrain); if (Robot.isSimulation()) { new MapleSim(drivetrain, collector, indexer, turret, hood, shooter); @@ -196,6 +198,7 @@ private void configureBindings() { new Trigger(copilot::getAButton).whileTrue(indexer.autoIndex(IndexerConstants.SPINDEXDER_POWER, IndexerConstants.TRANSFER_POWER)).onFalse(new InstantCommand(() -> indexer.stop())); new Trigger(copilot::getYButton).whileTrue(indexer.indexCommand(-0.5).withTimeout(0.1).andThen(indexer.indexCommand(1))); + new Trigger(() -> AllianceHelpers.isHubAboutToBeActive()).and(() -> drivetrain.isInZone()).whileTrue(shooter.runShootCommand(() -> ShooterConstants.VELOCITY_MAP.get(cannon.getTargetDistance()))); new Trigger(driver::getXButton).whileTrue(new InstantCommand(() -> drivetrain.resetPose(new Pose2d(12.566, 0.713, new Rotation2d(0.057))))); } diff --git a/src/main/java/frc/util/AllianceHelpers.java b/src/main/java/frc/util/AllianceHelpers.java index 16a44542..4f8e95c9 100644 --- a/src/main/java/frc/util/AllianceHelpers.java +++ b/src/main/java/frc/util/AllianceHelpers.java @@ -1,6 +1,7 @@ package frc.util; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; public class AllianceHelpers { @@ -18,7 +19,7 @@ public static boolean isRedAlliance() { return !isBlueAlliance(); } - public boolean isHubActive() { + public static boolean isHubActive() { // Hub is always enabled in autonomous. if (DriverStation.isAutonomousEnabled()) { return true; @@ -69,4 +70,56 @@ public boolean isHubActive() { return true; } } + + public static boolean isHubAboutToBeActive() { + // Hub is always enabled in autonomous. + if (DriverStation.isAutonomousEnabled()) { + return true; + } + + // At this point, if we're not teleop enabled, there is no hub. + if (!DriverStation.isTeleopEnabled()) { + return false; + } + + // We're teleop enabled, compute. + double matchTime = DriverStation.getMatchTime(); + String gameData = DriverStation.getGameSpecificMessage(); + + // If we have no game data, we cannot compute, assume hub is active, as its likely early in teleop. + if (gameData.isEmpty()) { + return true; + } + + boolean redInactiveFirst = false; + switch (gameData.charAt(0)) { + case 'R': + redInactiveFirst = true; + case 'B': + redInactiveFirst = false; + } + + // Shift one is active for blue if red won auto, or red if blue won auto. + boolean shift1Active = isBlueAlliance() ? redInactiveFirst : !redInactiveFirst; + + if (matchTime > 131) { + // Transition shift, hub is active. + return true; + } else if (matchTime > 106) { + // Shift 1 + return shift1Active; + } else if (matchTime > 81) { + // Shift 2 + return !shift1Active; + } else if (matchTime > 56) { + // Shift 3 + return shift1Active; + } else if (matchTime > 31) { + // Shift 4 + return !shift1Active; + } else { + // End game, hub always active. + return true; + } + } } \ No newline at end of file