511 keep flywheel spun up

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<Command> 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)))));
     }

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;
+        }
+    }
 }