新底盘

elastic-layout.json

@@ -91,6 +91,93 @@
               "period": 0.06,
               "sort_options": false
             }
+          },
+          {
+            "title": "HubActive",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/GameData2026/HubActive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "HubStatus",
+            "x": 256.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/GameData2026/HubStatus",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "SecondsUntilHubOpens",
+            "x": 0.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/GameData2026/SecondsUntilHubOpens",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shift",
+            "x": 256.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/GameData2026/Shift",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "FirstInactiveAlliance",
+            "x": 0.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/GameData2026/FirstInactiveAlliance",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "MatchTime",
+            "x": 256.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/GameData2026/MatchTime",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
           }
         ]
       }

src/main/java/frc/robot/Constants.java

@@ -6,18 +6,6 @@ import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.geometry.Pose2d;
 
 public final class Constants {
-    public static final class DemoMode {
-        public static final boolean ENABLED = true;
-
-        public static final double normalDriveScale = 0.12;
-        public static final double normalTurnScale = 0.20;
-        public static final double boostDriveScale = 0.25;
-        public static final double boostTurnScale = 0.30;
-
-        public static final double maxLinearSpeedMetersPerSecond = 2.4;
-        public static final double maxAngularRateRadiansPerSecond = 1.6;
-    }
-
     public static final boolean LEDUsing = false;
     public enum RobotStatus{
         Stopped,
@@ -29,7 +17,7 @@ public final class Constants {
         CrossingTrench
     }
     public class Shooter {
-        public static final double conveyorSpeed = 80;
+        public static final double conveyorSpeed = 150;
         public static final double conveyorSpeedForAuto = 10;
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
@@ -92,7 +80,7 @@ public final class Constants {
     public class Limelight {
         public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
         public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
-        public static final Boolean UsingMetaTag2 = false; // Use Limelight's own pose/yaw estimate instead of gyro-assisted MegaTag2.
+        public static final Boolean UsingMetaTag2 = true; // 是否使用 MetaTag2（如果你们的 pipeline 配置了这个功能的话）
         public static final int climbPipelineIndex = 8;
         public static final int locatePipelineIndex = 0;
 

src/main/java/frc/robot/Robot.java

@@ -23,6 +23,7 @@ public class Robot extends TimedRobot {
     public void robotPeriodic() {
         CommandScheduler.getInstance().run(); 
         m_robotContainer.tunerPeriodic();
+        m_robotContainer.dashboardPeriodic();
 
     }
 

src/main/java/frc/robot/RobotContainer.java

@@ -30,15 +30,16 @@ import frc.robot.utils.LedBindings;
 import frc.robot.controls.ClimberControls;
 import frc.robot.controls.Autocommand;
 import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.utils.GameData2026;
 public class RobotContainer {
 
   /* ====================== */
   /*         手柄            */
   /* ====================== */
 
-  // Demo mode uses one Xbox controller on port 0.
+  // 使用2个 Xbox 手柄（端口 0,1）
   private final CommandXboxController controllerlower = new CommandXboxController(0);
-  private final CommandXboxController controllerupper = controllerlower;
+  private final CommandXboxController controllerupper = new CommandXboxController(1);
 
   /* ====================== */
   /*         子系统           */
@@ -63,6 +64,7 @@ public class RobotContainer {
   //private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
   private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
   private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
+  private final GameData2026 gameData2026 = new GameData2026();
   private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
   private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
   private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
@@ -124,8 +126,6 @@ public class RobotContainer {
     // Disabled 时进入 idle（防止模块乱动）
     RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
 
-    if (!Constants.DemoMode.ENABLED) {
-
     
     //左保险：按住刹车
     controllerlower.leftBumper()
@@ -174,7 +174,6 @@ public class RobotContainer {
     controllerlower.a()
     .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
     .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
-    }
   }
 
   private void configueShooter(){
@@ -184,13 +183,7 @@ public class RobotContainer {
     controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
     controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
     controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
-    if (Constants.DemoMode.ENABLED) {
+    controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
-      controllerupper.rightTrigger()
-          .onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming))
-          .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
-    } else {
-      controllerupper.start().onTrue(shooterControls.resetBackboardCommand());
-    }
     
   }
 
@@ -199,9 +192,7 @@ public class RobotContainer {
   }
   
   public Command getAutonomousCommand() {
-    if (Constants.DemoMode.ENABLED) {
+
-      return Commands.none();
-    }
     return autoChooser.getSelected();
   }
 
@@ -213,6 +204,10 @@ public class RobotContainer {
     shooterSubsystem.setRightFollowLeft();
   }
 
+  public void dashboardPeriodic() {
+    gameData2026.periodic();
+  }
+
   // private void configueClimber() {
   //   climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
   // }

src/main/java/frc/robot/controls/DriveControls.java

@@ -107,18 +107,9 @@ public class DriveControls {
         SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
         double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
         double turnScale  = boostEnabled ? boostTurnScale  : normalTurnScale;
-        if (Constants.DemoMode.ENABLED) {
-            driveScale = boostEnabled ? Constants.DemoMode.boostDriveScale : Constants.DemoMode.normalDriveScale;
-            turnScale = boostEnabled ? Constants.DemoMode.boostTurnScale : Constants.DemoMode.normalTurnScale;
-        }
         double vx = -driver.getLeftY() * maxSpeed * driveScale;
         double vy = -driver.getLeftX() * maxSpeed * driveScale;
         double vomega = -driver.getRightX() * maxAngularRate * turnScale;
-        if (Constants.DemoMode.ENABLED) {
-            vx = clamp(vx, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
-            vy = clamp(vy, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
-            vomega = clamp(vomega, -Constants.DemoMode.maxAngularRateRadiansPerSecond, Constants.DemoMode.maxAngularRateRadiansPerSecond);
-        }
         // ====== execute 内替换 limiter：只限加速、不限减速 ======
         double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
         double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
@@ -139,15 +130,7 @@ public class DriveControls {
         autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
         autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
         
-        RobotStatus currentStatus = robotStatusManager.getStatus();
+        switch (robotStatusManager.getStatus()) {
-        if (Constants.DemoMode.ENABLED
-            && currentStatus != RobotStatus.Stopped
-            && currentStatus != RobotStatus.AllTelop
-            && currentStatus != RobotStatus.Climbing) {
-            currentStatus = RobotStatus.AllTelop;
-        }
-
-        switch (currentStatus) {
             case Stopped:
                 vx = 0;
                 vy = 0;
@@ -229,9 +212,8 @@ public class DriveControls {
         autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
         
         boolean isAutoLike =
-            !Constants.DemoMode.ENABLED && (
             robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench);
+            robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
 
         if (fieldCentricEnabled || isAutoLike) {
 
@@ -249,9 +231,8 @@ public class DriveControls {
                 .withRotationalRate(vomega);
         }
         }, () -> fieldCentricEnabled || 
-            (!Constants.DemoMode.ENABLED && (
             robotStatusManager.getStatus() == RobotStatus.CrossingBump || 
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench))
+            robotStatusManager.getStatus() == RobotStatus.CrossingTrench
              ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
     }
 
@@ -366,10 +347,6 @@ public class DriveControls {
         return limiter.calculate(target);
     }
 
-    private double clamp(double value, double min, double max) {
-        return Math.max(min, Math.min(max, value));
-    }
-
         /** 紧急停止：清零限速器，让速度输出立刻归零 */
     public void emergencyStop() {
         vxLimiter.reset(0.0);
@@ -439,9 +416,6 @@ public class DriveControls {
     //     }
     // }
     public Command autoAimCommand() {
-        if (Constants.DemoMode.ENABLED) {
-            return Commands.runOnce(drivetrain::stop, drivetrain);
-        }
         return drivetrain.applyRequest(() -> {
         double[] distanceAndRotation = calculateDistanceAndRotationToHub();
         double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
@@ -457,9 +431,6 @@ public class DriveControls {
         }, drivetrain));
     }
     public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
-    if (Constants.DemoMode.ENABLED) {
-        return Commands.runOnce(drivetrain::stop, drivetrain);
-    }
     Timer timer = new Timer();
 
     return Commands.sequence(

src/main/java/frc/robot/controls/IntakerControls.java

@@ -81,12 +81,6 @@ public class IntakerControls {
 
         IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
 
-        boolean demoModeEnabled = Constants.DemoMode.ENABLED;
-        if (demoModeEnabled) {
-            IntakerSubsystem.stopIntakerotater();
-            return;
-        }
-
         if(controller.getLeftY() < -0.5){
                     IntakerSubsystem.IntakerUpNonStop();
             } else if(controller.getLeftY() > 0.5){

src/main/java/frc/robot/controls/ShooterControls.java

@@ -48,7 +48,7 @@ public class ShooterControls {
                         shooterSubsystem.outputBackboard();
                     }  
                     shooterSubsystem.setFlywheelSpeedByRPS(0);
-                    if(!Constants.DemoMode.ENABLED && controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
+                    if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
                     else{shooterSubsystem.setConveyorSpeedByRPS(0);}
                     break;
                 case PassingBall:
@@ -81,11 +81,9 @@ public class ShooterControls {
                 case AutoAimming:
                     var x = shooterSubsystem.getDistanceToHub();
                     x = Math.max(1.2, Math.min(4.5, x));
-                    //flywheelSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
+                    flywheelSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
-                    flywheelSpeed = 60;
                     flywheelSpeed += flywheelSpeedOffset;
-                    //backboardPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
+                    backboardPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
-                    backboardPosition = 500;
                     backboardPosition += backboardPositionOffset;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {

src/main/java/frc/robot/generated/TunerConstants.java

@@ -24,14 +24,14 @@ public class TunerConstants {
     // The steer motor uses any SwerveModule.SteerRequestType control request with the
     // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
     private static final Slot0Configs steerGains = new Slot0Configs()
-        .withKP(70).withKI(0).withKD(0.5)
+        .withKP(100).withKI(0).withKD(0.5)
         .withKS(0.1).withKV(1.59).withKA(0)
         .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
     // When using closed-loop control, the drive motor uses the control
     // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
     private static final Slot0Configs driveGains = new Slot0Configs()
-        .withKP(0.01).withKI(0).withKD(0)
+        .withKP(0.1).withKI(0).withKD(0)
-        .withKS(0.05).withKV(0.117);
+        .withKS(0).withKV(0.124);
 
     // The closed-loop output type to use for the steer motors;
     // This affects the PID/FF gains for the steer motors
@@ -42,7 +42,7 @@ public class TunerConstants {
 
     // The type of motor used for the drive motor
     private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
-    // The type of motor used for the drive motor
+    // The type of motor used for the steer motor
     private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
 
     // The remote sensor feedback type to use for the steer motors;
@@ -55,7 +55,14 @@ public class TunerConstants {
 
     // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
     // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
-    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                // Default supply current limit is 70 A, but it can be lowered to avoid brownouts.
+                // Supply current limits can be larger than the breaker current rating.
+                .withSupplyCurrentLimit(Amps.of(70))
+                .withSupplyCurrentLimitEnable(true)
+        );
     private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
         .withCurrentLimits(
             new CurrentLimitsConfigs()
@@ -66,23 +73,14 @@ public class TunerConstants {
         );
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-    private static final Pigeon2Configuration pigeonConfigs =
+    private static final Pigeon2Configuration pigeonConfigs = null;
-    new Pigeon2Configuration()
-        .withGyroTrim(
-            new GyroTrimConfigs()
-                .withGyroScalarZ(-5.0)
-        )
-        .withPigeon2Features(
-            new Pigeon2FeaturesConfigs()
-                .withEnableCompass(false)
-        );
-
 
     // CAN bus that the devices are located on;
     // All swerve devices must share the same CAN bus
     public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot");
 
-    // Theoretical free speed (m/s) at 12 V applied output;
+    // Measured robot speed (m/s) at 12 V applied output;
+    // This is NOT the desired max robot speed - see MaxSpeed in RobotContainer instead;
     // This needs to be tuned to your individual robot
     public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
 
@@ -101,7 +99,7 @@ public class TunerConstants {
 
     // These are only used for simulation
     private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
-    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
+    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.035);
     // Simulated voltage necessary to overcome friction
     private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
     private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
@@ -136,10 +134,10 @@ public class TunerConstants {
 
 
     // Front Left
-    private static final int kFrontLeftDriveMotorId = 11;
+    private static final int kFrontLeftDriveMotorId = 1;
-    private static final int kFrontLeftSteerMotorId = 12;
+    private static final int kFrontLeftSteerMotorId = 2;
-    private static final int kFrontLeftEncoderId = 13;
+    private static final int kFrontLeftEncoderId = 3;
-    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.412841796875);
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.18115234375);
     private static final boolean kFrontLeftSteerMotorInverted = false;
     private static final boolean kFrontLeftEncoderInverted = false;
 
@@ -147,10 +145,10 @@ public class TunerConstants {
     private static final Distance kFrontLeftYPos = Inches.of(11.375);
 
     // Front Right
-    private static final int kFrontRightDriveMotorId = 21;
+    private static final int kFrontRightDriveMotorId = 11;
-    private static final int kFrontRightSteerMotorId = 22;
+    private static final int kFrontRightSteerMotorId = 12;
-    private static final int kFrontRightEncoderId = 23;
+    private static final int kFrontRightEncoderId = 13;
-    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.012939453125);
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.16162109375);
     private static final boolean kFrontRightSteerMotorInverted = false;
     private static final boolean kFrontRightEncoderInverted = false;
 
@@ -158,10 +156,10 @@ public class TunerConstants {
     private static final Distance kFrontRightYPos = Inches.of(-11.375);
 
     // Back Left
-    private static final int kBackLeftDriveMotorId = 1;
+    private static final int kBackLeftDriveMotorId = 21;
-    private static final int kBackLeftSteerMotorId = 2;
+    private static final int kBackLeftSteerMotorId = 22;
-    private static final int kBackLeftEncoderId = 3;
+    private static final int kBackLeftEncoderId = 23;
-    private static final Angle kBackLeftEncoderOffset = Rotations.of(0.07275390625);
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.017822265625);
     private static final boolean kBackLeftSteerMotorInverted = false;
     private static final boolean kBackLeftEncoderInverted = false;
 
@@ -172,7 +170,7 @@ public class TunerConstants {
     private static final int kBackRightDriveMotorId = 31;
     private static final int kBackRightSteerMotorId = 32;
     private static final int kBackRightEncoderId = 33;
-    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4892578125);
+    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.490234375);
     private static final boolean kBackRightSteerMotorInverted = false;
     private static final boolean kBackRightEncoderInverted = false;
 
@@ -203,7 +201,7 @@ public class TunerConstants {
 
     /**
      * Creates a CommandSwerveDrivetrain instance.
-     * This should only be called once in your robot program,.
+     * This should only be called once in your robot program.
      */
     public static CommandSwerveDrivetrain createDrivetrain() {
         return new CommandSwerveDrivetrain(

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -162,9 +162,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
     
     public void forceUsingLimelightmt2() {
-        if (Constants.DemoMode.ENABLED) {
-            return;
-        }
         LimelightHelpers.PoseEstimate mt2 =
         LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
 
@@ -177,9 +174,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
     
     public void forceUsingLimelightmt2WithllName(String llString) {
-        if (Constants.DemoMode.ENABLED) {
-            return;
-        }
         LimelightHelpers.PoseEstimate mt2 =
         LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
 
@@ -432,16 +426,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         Pose2d pose = getState().Pose;
         m_field.setRobotPose(pose);
         posePub.set(pose);
-        LimelightHelpers.PoseEstimate shooterEstimate =
+        shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose);
-            LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+        intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose);
-        LimelightHelpers.PoseEstimate intakerEstimate =
-            LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-        if (shooterEstimate != null && shooterEstimate.tagCount > 0) {
-            shooter_ll_field.setRobotPose(shooterEstimate.pose);
-        }
-        if (intakerEstimate != null && intakerEstimate.tagCount > 0) {
-            intaker_ll_field.setRobotPose(intakerEstimate.pose);
-        }
         driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
         driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
         driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
@@ -462,9 +448,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
             0, 0, 0, 0, 0
         );
 
-        if (!Constants.DemoMode.ENABLED) {
+        fuseLimelightmt2();
-            fuseLimelightmt2();
-        }
 
         if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
             DriverStation.getAlliance().ifPresent(allianceColor -> {

src/main/java/frc/robot/utils/GameData2026.java

@@ -0,0 +1,117 @@
+package frc.robot.utils;
+
+import java.util.Optional;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public class GameData2026 {
+    private static final double SHIFT_1_END_TIME = 105.0;
+    private static final double SHIFT_2_END_TIME = 80.0;
+    private static final double SHIFT_3_END_TIME = 55.0;
+    private static final double SHIFT_4_END_TIME = 30.0;
+    private static final double TRANSITION_END_TIME = 130.0;
+
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("GameData2026");
+
+    public void periodic() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        double matchTime = DriverStation.getMatchTime();
+        String gameData = DriverStation.getGameSpecificMessage();
+
+        HubWindow window = calculateHubWindow(alliance, matchTime, gameData);
+
+        table.getEntry("HubActive").setBoolean(window.hubActive);
+        table.getEntry("HubStatus").setString(window.status);
+        table.getEntry("SecondsUntilHubOpens").setDouble(window.secondsUntilHubOpens);
+        table.getEntry("SecondsUntilHubChanges").setDouble(window.secondsUntilHubChanges);
+        table.getEntry("Shift").setString(window.shift);
+        table.getEntry("MatchTime").setDouble(matchTime);
+        table.getEntry("GameData").setString(gameData);
+        table.getEntry("Alliance").setString(alliance.map(Enum::name).orElse("Unknown"));
+        table.getEntry("FirstInactiveAlliance").setString(getFirstInactiveAlliance(gameData));
+    }
+
+    private HubWindow calculateHubWindow(Optional<Alliance> alliance, double matchTime, String gameData) {
+        if (alliance.isEmpty()) {
+            return new HubWindow(false, "NO_ALLIANCE", "Disabled", Double.NaN, Double.NaN);
+        }
+
+        if (DriverStation.isAutonomousEnabled()) {
+            return new HubWindow(true, "OPEN", "Auto", 0.0, Double.NaN);
+        }
+
+        if (!DriverStation.isTeleopEnabled()) {
+            return new HubWindow(false, "DISABLED", "Disabled", Double.NaN, Double.NaN);
+        }
+
+        if (matchTime < 0.0) {
+            return new HubWindow(true, "OPEN_TIME_UNKNOWN", "Unknown", 0.0, Double.NaN);
+        }
+
+        Boolean redInactiveFirst = getRedInactiveFirst(gameData);
+        if (redInactiveFirst == null) {
+            return new HubWindow(true, "WAITING_DATA", "WaitingData", 0.0, Double.NaN);
+        }
+
+        boolean shift1Active = switch (alliance.get()) {
+            case Red -> !redInactiveFirst;
+            case Blue -> redInactiveFirst;
+        };
+
+        if (matchTime > TRANSITION_END_TIME) {
+            return new HubWindow(true, "OPEN", "Transition", 0.0, matchTime - TRANSITION_END_TIME);
+        } else if (matchTime > SHIFT_1_END_TIME) {
+            return fromShift("Shift1", shift1Active, matchTime, SHIFT_1_END_TIME);
+        } else if (matchTime > SHIFT_2_END_TIME) {
+            return fromShift("Shift2", !shift1Active, matchTime, SHIFT_2_END_TIME);
+        } else if (matchTime > SHIFT_3_END_TIME) {
+            return fromShift("Shift3", shift1Active, matchTime, SHIFT_3_END_TIME);
+        } else if (matchTime > SHIFT_4_END_TIME) {
+            return fromShift("Shift4", !shift1Active, matchTime, SHIFT_4_END_TIME);
+        } else {
+            return new HubWindow(true, "OPEN", "Endgame", 0.0, Math.max(matchTime, 0.0));
+        }
+    }
+
+    private HubWindow fromShift(String shift, boolean hubActive, double matchTime, double nextBoundary) {
+        double secondsUntilChange = Math.max(matchTime - nextBoundary, 0.0);
+        double secondsUntilOpen = hubActive ? 0.0 : secondsUntilChange;
+        String status = hubActive ? "OPEN" : "CLOSED";
+        return new HubWindow(hubActive, status, shift, secondsUntilOpen, secondsUntilChange);
+    }
+
+    private Boolean getRedInactiveFirst(String gameData) {
+        if (gameData.isEmpty()) {
+            return null;
+        }
+
+        return switch (gameData.charAt(0)) {
+            case 'R' -> true;
+            case 'B' -> false;
+            default -> null;
+        };
+    }
+
+    private String getFirstInactiveAlliance(String gameData) {
+        if (gameData.isEmpty()) {
+            return "None";
+        }
+
+        return switch (gameData.charAt(0)) {
+            case 'R' -> "Red";
+            case 'B' -> "Blue";
+            default -> "Invalid";
+        };
+    }
+
+    private record HubWindow(
+        boolean hubActive,
+        String status,
+        String shift,
+        double secondsUntilHubOpens,
+        double secondsUntilHubChanges
+    ) {}
+}