季后赛重构：补完 Superstructure 状态机与射击门控

- Superstructure 增加 INDEXING/HOLDING_NOTE 状态，实现完整状态流转：
  DEPLOYING_INTAKE -> INTAKING -> INDEXING -> HOLDING_NOTE ->
  SPINNING_UP -> AIMING -> READY_TO_SHOOT -> SHOOTING -> CLEANUP
- canShoot 门控补全：atSpeed && isAimed && rotaterAtGoal && hasNote && 底盘近静止
  （hasNote/静止门控均可在 Constants 中旁路）
- 无传感器持球跟踪：INDEXING 完成置位、射击/EJECT 后清空，
  未归零被拦下的 intake 不会误置持球
- DriveControls 在既有 hub 方位角计算处同步 setTargetHeading，
  使 isAimed 门控与 AutoAimming 实际瞄准同源；自动阶段经 pushDistanceData 保持更新
- 移除 Superstructure 基于 limelight tx 的瞄准更新（符号错误且无目标时滞留旧值）
- 背板回零命令下沉到 ShooterSubsystem，OperatorControls 不再直接操作电机
- ShooterSubsystem 清理旧别名与闭环遗留接口，背板独立控制请求
- 射表改为 InterpolatingDoubleTreeMap（Constants.Shooter.flywheelRpsByDistance）
- IntakeRotater：未归零时 atGoal 一律 false，新增 blockedByNotZeroed 遥测；
  atGoal 容差改为非零占位值
- 修复 gradlew 缺少可执行权限

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01YTEBmjtkz1ouCmn5yytFti

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

@@ -3,6 +3,7 @@ package frc.robot;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 
 public final class Constants {
     private Constants() {}
@@ -33,6 +34,16 @@ public final class Constants {
         public static final double flywheelSpeedToleranceRps = 5.0;
         public static final double defaultFlywheelTargetRps = 64.0;
 
+        // 距离(米) -> 飞轮转速(RPS) 射表。当前为占位点；
+        // 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。
+        public static final InterpolatingDoubleTreeMap flywheelRpsByDistance = new InterpolatingDoubleTreeMap();
+        static {
+            flywheelRpsByDistance.put(1.2, 58.8);
+            flywheelRpsByDistance.put(2.5, 64.0);
+            flywheelRpsByDistance.put(3.5, 68.0);
+            flywheelRpsByDistance.put(5.0, 74.0);
+        }
+
         public static final double conveyorSpeed = 150;
         public static final double conveyorSpeedForAuto = 10;
 
@@ -143,7 +154,9 @@ public final class Constants {
             .withKV(0.00).withKA(0.00).withKS(0.00);
 
         public static final double IntakeGetterSpeedforAuto = 0.5;
-        public static final double positionToleranceRotations = 0.00;
+        // atGoal 判定容差。为 0 会导致 atGoal 永远为 false、射击门控永远不通过；
+        // 这里给出可用的占位值，真车调好位置闭环后再收紧。
+        public static final double positionToleranceRotations = 0.3;
     }
 
     public static class Conveyor {
@@ -161,5 +174,15 @@ public final class Constants {
         public static final double shootTimeoutSeconds = 1.0;
         public static final double aimToleranceDegrees = 1.5;
         public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
+
+        // 松开 intake 键后，INDEXING 状态继续收纳球的时长。
+        public static final double indexingSeconds = 0.5;
+        // 射击时背板的目标位置（外接编码器计数）。
+        public static final double backboardShootPosition = 175.0;
+        // 没有球检测传感器，持球状态由状态机跟踪推断。
+        // 若跟踪不可靠导致射击被卡，可临时置 false 旁路 hasNote 门控。
+        public static final boolean hasNoteGateEnabled = true;
+        // 射击门控是否要求底盘接近静止。
+        public static final boolean stationaryGateEnabled = true;
     }
 }

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

@@ -3,12 +3,9 @@
 // 修改和/或分享本代码。
 
 package frc.robot;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.subsystems.ShooterSubsystem;
 
 public class Robot extends TimedRobot {
     private Command m_autonomousCommand;

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

@@ -80,9 +80,9 @@ public class RobotContainer {
   private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
   private final GameData2026 gameData2026 = new GameData2026();
   private ConfigTalonFXMotorTuner configFlywheelTuner =
-      new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
+      new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
   private ConfigTalonFXMotorTuner configConveyorTuner =
-      new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
+      new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
   private ConfigTalonFXSMotorTuner configBackboardTuner =
       new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
 

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

@@ -129,6 +129,10 @@ public class DriveControls {
         autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
         autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
         autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
+        // 把朝向 hub 的目标角同步给底盘，供 Superstructure 读取 isAimed() 做射击门控；
+        // 数据来源与 AutoAimming 自动旋转完全相同，保证门控和实际瞄准一致。
+        drivetrain.setTargetHeading(
+            drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
 
         switch (robotStatusManager.getStatus()) {
             case Stopped:
@@ -472,6 +476,10 @@ public class DriveControls {
         // 将距离值写入 NetworkTable
         autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
         autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
+        // 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新，
+        // 让自动程序里的射击门控 isAimed() 有效。
+        drivetrain.setTargetHeading(
+            drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
     }
     
 }

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

@@ -41,6 +41,10 @@ public class OperatorControls {
         return superstructure.requestEjectCommand();
     }
 
+    public Command requestManualCommand() {
+        return superstructure.requestManualCommand();
+    }
+
     public void adjustFlywheelSpeedOffset(double offset) {
         superstructure.adjustFlywheelSpeedOffset(offset);
     }
@@ -73,21 +77,9 @@ public class OperatorControls {
         );
     }
 
-    // 保留旧背板归零/回零命令，方便调参和现场恢复。
+    // 保留旧背板回零动作，方便调参和现场恢复；
+    // 具体电机操作在 ShooterSubsystem 内实现，这里只转发命令。
     public Command resetBackboard0Command() {
-        return Commands.sequence(
+        return shooterSubsystem.homeBackboardCommand();
-            Commands.runOnce(() -> shooterSubsystem.setBackboardPosition(0.0), shooterSubsystem),
-            Commands.run(
-                () -> {
-                    if (shooterSubsystem.isBackboardAtTarget()) {
-                        shooterSubsystem.backboardMotor.set(0.0);
-                    } else {
-                        shooterSubsystem.outputBackboard();
-                    }
-                },
-                shooterSubsystem
-            ).until(shooterSubsystem::isBackboardAtTarget),
-            Commands.runOnce(() -> shooterSubsystem.backboardMotor.set(0.0), shooterSubsystem)
-        );
     }
 }

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

@@ -12,7 +12,8 @@ import frc.robot.Constants;
 /**
  * 主 conveyor 的开环控制封装。
  *
- * 本赛季没有球检测传感器，因此这里仅报告电机状态；
+ * 本赛季没有球检测传感器，持球状态由 Superstructure 按状态流程推断后
+ * 写入这里（INDEXING 完成置位，射击/EJECT 后清空）；
  * 所有动作顺序判断都放在 Superstructure。
  */
 public class ConveyorSubsystem extends SubsystemBase {
@@ -22,6 +23,7 @@ public class ConveyorSubsystem extends SubsystemBase {
     private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
 
     private double voltage = 0.0;
+    private boolean notePresent = false;
 
     public ConveyorSubsystem() {
         mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
@@ -52,9 +54,18 @@ public class ConveyorSubsystem extends SubsystemBase {
         mainConveyor.setControl(voltageRequest.withOutput(voltage));
     }
 
+    public boolean hasNote() {
+        return notePresent;
+    }
+
+    public void setNotePresent(boolean present) {
+        notePresent = present;
+    }
+
     @Override
     public void periodic() {
         table.getEntry("voltage").setDouble(voltage);
         table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
+        table.getEntry("hasNote").setBoolean(notePresent);
     }
 }

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

@@ -84,9 +84,19 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
     }
 
     public boolean atGoal() {
+        // 未归零时编码器读数没有意义，一律视为未到位，
+        // 让上层射击门控和状态流转不会被假读数放行。
+        if (!zeroed) {
+            return false;
+        }
         return Math.abs(getPosition() - goal.rotations) <= Constants.Intaker.positionToleranceRotations;
     }
 
+    public boolean isBlockedByNotZeroed() {
+        // 归零前收到了 SAFE 以外的位置请求：机构被安全逻辑拦下，需要先归零。
+        return !zeroed && goal != IntakePosition.SAFE;
+    }
+
     public void zeroPosition() {
         // 只有 intake 物理上处于已知零点姿态时才调用此方法。
         leftIntakeRotater.setPosition(0.0);
@@ -138,6 +148,7 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
     public void periodic() {
         table.getEntry("goal").setString(goal.name());
         table.getEntry("zeroed").setBoolean(zeroed);
+        table.getEntry("blockedByNotZeroed").setBoolean(isBlockedByNotZeroed());
         table.getEntry("position").setDouble(getPosition());
         table.getEntry("atGoal").setBoolean(atGoal());
         table.getEntry("manualVoltage").setDouble(manualVoltage);

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

@@ -48,8 +48,7 @@ public class LimelightSubsystem extends SubsystemBase {
     }
 
     public double getShooterSetpointByDistance() {
-        // 初始测试用临时映射；飞轮和背板调好后应替换为实测射击表。
+        // 查 Constants 里的插值射表；真车试射后往表里加实测点即可。
-        double distance = getDistanceToTarget();
+        return Constants.Shooter.flywheelRpsByDistance.get(getDistanceToTarget());
-        return Constants.Shooter.defaultFlywheelTargetRps + (distance - 2.5) * 4.0;
     }
 }

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

@@ -16,14 +16,18 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.Encoder;
+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;
 
 /**
  * Shooter 总成的硬件封装。
  *
- * 飞轮和 shooter 内部 conveyor 都使用电压开环输出。
+ * 飞轮和 shooter 内部 conveyor 都使用电压开环输出，
+ * 右侧电机通过反向 follower 跟随左侧电机。
  * 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。
+ * 何时 spin up、何时喂球由 Superstructure 决定，本类不做比赛动作判断。
  */
 public class ShooterSubsystem extends SubsystemBase {
     public final TalonFX leftFlywheel;
@@ -31,17 +35,13 @@ public class ShooterSubsystem extends SubsystemBase {
     public final TalonFX leftConveyor;
     public final TalonFX rightConveyor;
 
-    // 保留旧字段别名，让原有 dashboard tuner 在重构后仍能指向左飞轮和左 conveyor。
-    public final TalonFX flywheelMotorLeft;
-    public final TalonFX conveyorMotor;
-
     public final TalonFXS backboardMotor;
     public final Encoder backboardEncoder;
     public final ProfiledPIDController backboardPID;
 
     private final VoltageOut flywheelVoltageRequest = new VoltageOut(0);
     private final VoltageOut conveyorVoltageRequest = new VoltageOut(0);
-    private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
+    private final VelocityVoltage backboardVelocityRequest = new VelocityVoltage(0);
 
     private final Follower flywheelFollower;
     private final Follower conveyorFollower;
@@ -59,9 +59,6 @@ public class ShooterSubsystem extends SubsystemBase {
         leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID);
         rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID);
 
-        flywheelMotorLeft = leftFlywheel;
-        conveyorMotor = leftConveyor;
-
         leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
         rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
         leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
@@ -95,6 +92,10 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
     }
 
+    /* ====================== */
+    /*         飞轮            */
+    /* ====================== */
+
     public void setRightFollowLeft() {
         // 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
         rightFlywheel.setControl(flywheelFollower);
@@ -120,16 +121,11 @@ public class ShooterSubsystem extends SubsystemBase {
     }
 
     public void setFlywheelVelocityByDistance(double distance) {
-        // 临时距离映射。拿到真实射击数据后，应替换为实测表或重新调常量。
+        // 查 Constants 里的射表，两端自动取边界值。
-        double clampedDistance = MathUtil.clamp(distance, 1.2, 5.0);
+        double targetRps = Constants.Shooter.flywheelRpsByDistance.get(distance);
-        double targetRps = Constants.Shooter.defaultFlywheelTargetRps + (clampedDistance - 2.5) * 4.0;
         setFlywheelVelocity(targetRps, targetRps);
     }
 
-    public void setFlywheelSpeedByRPS(double targetSpeed) {
-        setFlywheelVelocity(targetSpeed, targetSpeed);
-    }
-
     public void setFlywheelVoltage(double volts) {
         targetLeftFlywheelRps = 0.0;
         targetRightFlywheelRps = 0.0;
@@ -138,39 +134,10 @@ public class ShooterSubsystem extends SubsystemBase {
         setRightFollowLeft();
     }
 
-    public void runShooterConveyor(double percent) {
-        shooterConveyorPercent = MathUtil.clamp(percent, -1.0, 1.0);
-        leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorPercent * 12.0));
-        setRightFollowLeft();
-    }
-
-    public void stopShooterConveyor() {
-        runShooterConveyor(0.0);
-    }
-
-    public void setConveyorSpeedByRPS(double speed) {
-        leftConveyor.setControl(velocityRequest.withVelocity(speed));
-        setRightFollowLeft();
-    }
-
-    public void setConveyorSpeedOpen(double speed) {
-        runShooterConveyor(speed);
-    }
-
     public void stopFlywheel() {
         setFlywheelVoltage(Constants.Shooter.flywheelIdleVoltage);
     }
 
-    public void stopAll() {
-        stopFlywheel();
-        stopShooterConveyor();
-        backboardMotor.set(0);
-    }
-
-    public void stopMotors() {
-        stopAll();
-    }
-
     public boolean atSpeed() {
         // 虽然飞轮控制是电压开环，但仍读取 TalonFX 速度作为喂球门控。
         if (targetFlywheelRps <= 1.0) {
@@ -200,13 +167,35 @@ public class ShooterSubsystem extends SubsystemBase {
         return targetFlywheelRps;
     }
 
+    /* ====================== */
+    /*    shooter conveyor     */
+    /* ====================== */
+
+    public void runShooterConveyor(double percent) {
+        shooterConveyorPercent = MathUtil.clamp(percent, -1.0, 1.0);
+        leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorPercent * 12.0));
+        setRightFollowLeft();
+    }
+
+    public void stopShooterConveyor() {
+        runShooterConveyor(0.0);
+    }
+
+    public double getConveyorStatorCurrent() {
+        return leftConveyor.getStatorCurrent().getValueAsDouble();
+    }
+
+    /* ====================== */
+    /*         背板            */
+    /* ====================== */
+
     public void setBackboardSpeedByRPS(double speed) {
         if (speed > 0 && speed < 10) {
             speed = 10;
         } else if (speed < 0 && speed > -10) {
             speed = -10;
         }
-        backboardMotor.setControl(velocityRequest.withVelocity(speed));
+        backboardMotor.setControl(backboardVelocityRequest.withVelocity(speed));
     }
 
     public void setBackboardPosition(double targetPosition) {
@@ -232,6 +221,20 @@ public class ShooterSubsystem extends SubsystemBase {
         setBackboardSpeedByRPS(pidOutput);
     }
 
+    public void holdBackboardAt(double targetPosition) {
+        // 上层只给目标位置；到位后停止输出，未到位时继续闭环。
+        setBackboardPosition(targetPosition);
+        if (isBackboardAtTarget()) {
+            backboardMotor.set(0.0);
+        } else {
+            outputBackboard();
+        }
+    }
+
+    public void stopBackboard() {
+        backboardMotor.set(0.0);
+    }
+
     public double getBackboardPosition() {
         return backboardEncoder.getDistance();
     }
@@ -240,28 +243,31 @@ public class ShooterSubsystem extends SubsystemBase {
         return backboardPID.atGoal();
     }
 
-    public void conveyorWaitForAcceleration() {
-        stopShooterConveyor();
-    }
-
-    public void conveyorRun() {
-        runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
-    }
-
     public void resetbackboardencoder() {
         backboardEncoder.reset();
+        backboardPID.reset(0);
     }
 
-    public double getConveyorStatorCurrent() {
+    /**
-        return leftConveyor.getStatorCurrent().getValueAsDouble();
+     * 背板回零命令：闭环回到 0 位后停止输出。
+     * 命令留在 subsystem 内，controls 层只负责绑定按键。
+     */
+    public Command homeBackboardCommand() {
+        return Commands.sequence(
+            Commands.runOnce(() -> setBackboardPosition(0.0), this),
+            Commands.run(() -> holdBackboardAt(0.0), this).until(this::isBackboardAtTarget),
+            Commands.runOnce(this::stopBackboard, this)
+        );
     }
 
-    public double getDistanceToHub() {
+    /* ====================== */
-        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
+    /*         整体            */
-    }
+    /* ====================== */
 
-    public double getDistanceToPassball() {
+    public void stopAll() {
-        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
+        stopFlywheel();
+        stopShooterConveyor();
+        stopBackboard();
     }
 
     @Override
@@ -270,6 +276,7 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("flywheelSpeed").setDouble(getAverageFlywheelVelocity());
         shooterNetworkTable.getEntry("leftFlywheelSpeed").setDouble(getLeftFlywheelVelocity());
         shooterNetworkTable.getEntry("rightFlywheelSpeed").setDouble(getRightFlywheelVelocity());
+        shooterNetworkTable.getEntry("flywheelSpeedDifference").setDouble(getFlywheelVelocityDifference());
         shooterNetworkTable.getEntry("flywheelTargetSpeed").setDouble(targetFlywheelRps);
         shooterNetworkTable.getEntry("flywheelAtSpeed").setBoolean(atSpeed());
         shooterNetworkTable.getEntry("shooterConveyorPercent").setDouble(shooterConveyorPercent);
@@ -277,8 +284,6 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
         shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
         shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
-        shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub());
-        shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
         shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
     }
 }

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

@@ -1,6 +1,6 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -9,14 +9,17 @@ 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 frc.robot.subsystems.IntakeRotaterSubsystem.IntakePosition;
 
 /**
  * 统一协调所有得分相关机构。
  *
  * subsystem 只暴露硬件级动作，OperatorControls 只表达操作员意图；
  * shooter、intake、conveyor、底盘瞄准和 Limelight 距离如何配合，
- * 统一由这里决定。
+ * 统一由这里决定。底盘瞄准本身仍由 DriveControls 的 AutoAimming
+ * 模式完成，这里只读取 drive.isAimed() 等状态，不直接控制底盘电机。
+ *
+ * 没有球检测传感器，持球状态按状态流程推断：INDEXING 完成置位，
+ * 射击/EJECT 后清空；跟踪不可靠时可用 Constants 里的开关旁路门控。
  */
 public class Superstructure extends SubsystemBase {
     // 操作员或自动程序请求的目标状态。
@@ -35,6 +38,8 @@ public class Superstructure extends SubsystemBase {
         STOWED,
         DEPLOYING_INTAKE,
         INTAKING,
+        INDEXING,
+        HOLDING_NOTE,
         SPINNING_UP,
         AIMING,
         READY_TO_SHOOT,
@@ -74,6 +79,10 @@ public class Superstructure extends SubsystemBase {
         this.limelight = limelight;
     }
 
+    /* ====================== */
+    /*     操作员意图入口       */
+    /* ====================== */
+
     public void requestIdle() {
         wantedState = WantedState.IDLE;
     }
@@ -130,12 +139,30 @@ public class Superstructure extends SubsystemBase {
         return systemState;
     }
 
+    /* ====================== */
+    /*        射击门控          */
+    /* ====================== */
+
     public boolean canShoot() {
-        // 本赛季不使用球检测传感器，因此射击门控只检查机器人能实际测量的条件。
+        boolean noteGate = !Constants.Superstructure.hasNoteGateEnabled || conveyor.hasNote();
+        boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary();
         return shooter.atSpeed()
-            && drive.isAimed();
+            && drive.isAimed()
+            && intakeRotater.atGoal()
+            && noteGate
+            && stationaryGate;
     }
 
+    private boolean isDriveStationary() {
+        ChassisSpeeds speeds = drive.getChassisSpeeds();
+        double translationSpeed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+        return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond;
+    }
+
+    /* ====================== */
+    /*      现场微调 offset     */
+    /* ====================== */
+
     public void adjustFlywheelSpeedOffset(double offsetRps) {
         flywheelSpeedOffsetRps += offsetRps;
     }
@@ -153,6 +180,10 @@ public class Superstructure extends SubsystemBase {
         shooter.resetbackboardencoder();
     }
 
+    /* ====================== */
+    /*        状态机主体        */
+    /* ====================== */
+
     @Override
     public void periodic() {
         // 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。
@@ -163,8 +194,6 @@ public class Superstructure extends SubsystemBase {
             return;
         }
 
-        updateAimingSetpoint();
-
         switch (wantedState) {
             case IDLE -> handleIdle();
             case INTAKE -> handleIntake();
@@ -178,15 +207,68 @@ public class Superstructure extends SubsystemBase {
     }
 
     private void handleIdle() {
-        // 安全默认状态：传球不动，intake 收回，背板保持受控。
+        // 从其他流程回到 IDLE 时的入口流转。
-        conveyor.hold();
+        switch (systemState) {
-        intakeRoller.stop();
+            // 松开 intake 键后先收纳，不直接回 STOWED。
-        intakeRotater.stop();
+            // 未归零被安全逻辑拦下时滚轮从未启动，不可能有球，直接回 STOWED，
+            // 避免把 hasNote 误置为 true。
+            case DEPLOYING_INTAKE, INTAKING -> setSystemState(
+                intakeRotater.isBlockedByNotZeroed() ? SystemState.STOWED : SystemState.INDEXING);
+            // 喂球中途松开按键：球已经交给飞轮，按射完处理。
+            case SHOOTING -> {
+                conveyor.setNotePresent(false);
+                setSystemState(SystemState.CLEANUP);
+            }
+            // EJECT 结束：球路已排空。
+            case EJECTING -> {
+                conveyor.setNotePresent(false);
+                setSystemState(SystemState.STOWED);
+            }
+            default -> {}
+        }
+
+        if (systemState == SystemState.INDEXING) {
+            // 收纳中：intake 回 HANDOFF 交接位，滚轮低压保持，conveyor 继续送球。
+            intakeRotater.moveToHandoff();
+            intakeRoller.hold();
+            conveyor.feedToShooter();
             shooter.stopFlywheel();
             shooter.stopShooterConveyor();
-        holdBackboardAtZero();
+            shooter.holdBackboardAt(0.0);
 
-        setSystemState(SystemState.STOWED);
+            if (timeInState() > Constants.Superstructure.indexingSeconds) {
+                conveyor.setNotePresent(true);
+                setSystemState(SystemState.HOLDING_NOTE);
+            }
+            return;
+        }
+
+        if (systemState == SystemState.CLEANUP) {
+            intakeRoller.stop();
+            conveyor.stop();
+            shooter.stopShooterConveyor();
+            shooter.stopFlywheel();
+            shooter.holdBackboardAt(0.0);
+            intakeRotater.stow();
+
+            if (timeInState() > Constants.Superstructure.cleanupSeconds) {
+                setSystemState(conveyor.hasNote() ? SystemState.HOLDING_NOTE : SystemState.STOWED);
+            }
+            return;
+        }
+
+        // 常规空闲姿态：有球时保持球位，无球时全部停止。
+        setSystemState(conveyor.hasNote() ? SystemState.HOLDING_NOTE : SystemState.STOWED);
+        intakeRotater.stow();
+        intakeRoller.stop();
+        if (conveyor.hasNote()) {
+            conveyor.hold();
+        } else {
+            conveyor.stop();
+        }
+        shooter.stopFlywheel();
+        shooter.stopShooterConveyor();
+        shooter.holdBackboardAt(0.0);
     }
 
     private void handleIntake() {
@@ -197,20 +279,24 @@ public class Superstructure extends SubsystemBase {
 
         shooter.stopFlywheel();
         shooter.stopShooterConveyor();
+        shooter.holdBackboardAt(0.0);
         intakeRotater.deployToGround();
-        intakeRoller.intake();
-        conveyor.feedToShooter();
-        holdBackboardAtZero();
 
-        if (intakeRotater.atGoal()) {
+        if (intakeRotater.isBlockedByNotZeroed()) {
-            setSystemState(SystemState.INTAKING);
+            // 旋转机构未归零，安全逻辑拦下展开动作；
+            // 滚轮和 conveyor 也不启动，等待操作员先执行归零（A 键）。
+            intakeRoller.stop();
+            conveyor.stop();
+            return;
         }
 
-        // 本赛季没有球检测，因此 intake 通过时间退出。
+        // 展开过程中即允许进球，位置到位后进入 INTAKING。
-        // 真车确认 roller/conveyor 速度后，需要现场调整这个时间。
+        // 没有球检测，收球何时结束由操作员松开按键决定（IDLE -> INDEXING）。
-        if (systemState == SystemState.INTAKING && timeInState() > 1.0) {
+        intakeRoller.intake();
-            wantedState = WantedState.IDLE;
+        conveyor.feedToShooter();
-            setSystemState(SystemState.STOWED);
+
+        if (systemState == SystemState.DEPLOYING_INTAKE && intakeRotater.atGoal()) {
+            setSystemState(SystemState.INTAKING);
         }
     }
 
@@ -220,63 +306,82 @@ public class Superstructure extends SubsystemBase {
             && systemState != SystemState.READY_TO_SHOOT
             && systemState != SystemState.SHOOTING
             && systemState != SystemState.CLEANUP) {
+            // 从 intake 流程直接转射击：没有传感器，默认球已吸入。
+            if (systemState == SystemState.DEPLOYING_INTAKE
+                || systemState == SystemState.INTAKING
+                || systemState == SystemState.INDEXING) {
+                conveyor.setNotePresent(true);
+            }
             setSystemState(SystemState.SPINNING_UP);
         }
 
+        // 射击姿态：intake 到 HANDOFF 让出球路，滚轮低压保持球位。
         intakeRotater.moveToHandoff();
         intakeRoller.hold();
-        conveyor.hold();
+
-        shooter.stopShooterConveyor();
         // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
         // 但不会绕过 Superstructure 状态机。
         double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
         shooter.setFlywheelVelocity(targetRps, targetRps);
-        holdBackboardForShot();
+        shooter.holdBackboardAt(Constants.Superstructure.backboardShootPosition + backboardPositionOffset);
-
-        if (shooter.atSpeed()) {
-            setSystemState(SystemState.AIMING);
-        }
-
-        if (shooter.atSpeed() && drive.isAimed() && intakeRotater.atGoal()) {
-            setSystemState(SystemState.READY_TO_SHOOT);
-        }
-
-        if (feedWhenReady && canShoot()) {
-            setSystemState(SystemState.SHOOTING);
-        }
 
         if (systemState == SystemState.SHOOTING) {
-            // 只有 canShoot() 通过后才允许喂球。
+            // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
+            // 避免球接触飞轮导致的掉速中断喂球。
             conveyor.feedToShooter();
             shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
 
             if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
+                conveyor.setNotePresent(false);
                 setSystemState(SystemState.CLEANUP);
             }
+            return;
         }
 
         if (systemState == SystemState.CLEANUP) {
             conveyor.stop();
             shooter.stopShooterConveyor();
             if (timeInState() > Constants.Superstructure.cleanupSeconds) {
+                // 一次按键完成一次射击；再射需要重新请求。
                 wantedState = WantedState.IDLE;
-                setSystemState(SystemState.STOWED);
+                setSystemState(conveyor.hasNote() ? SystemState.HOLDING_NOTE : SystemState.STOWED);
             }
+            return;
+        }
+
+        // 未开始喂球前，球路保持静止。
+        conveyor.hold();
+        shooter.stopShooterConveyor();
+
+        // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
+        if (canShoot()) {
+            setSystemState(SystemState.READY_TO_SHOOT);
+        } else if (shooter.atSpeed()) {
+            setSystemState(SystemState.AIMING);
+        } else {
+            setSystemState(SystemState.SPINNING_UP);
+        }
+
+        if (feedWhenReady && systemState == SystemState.READY_TO_SHOOT) {
+            setSystemState(SystemState.SHOOTING);
         }
     }
 
     private void handleEject() {
-        // Eject 会反转整条球路，但飞轮保持停止。
+        // Eject 反转整条球路，飞轮保持停止。
+        // 旋转机构未归零时 setGoal 内部会自行拦下，滚轮反转不受影响。
         setSystemState(SystemState.EJECTING);
         intakeRotater.deployToGround();
         intakeRoller.outtake();
         conveyor.reverse();
         shooter.runShooterConveyor(Constants.Superstructure.shooterReversePercent);
         shooter.stopFlywheel();
-        holdBackboardAtZero();
+        shooter.holdBackboardAt(0.0);
     }
 
     private void handleManual() {
+        // 手动模式：状态机不再驱动机构，交给现场手动/调参工具；
+        // 只保持飞轮与喂球停止，避免意外射出。
         setSystemState(SystemState.MANUAL_OVERRIDE);
         shooter.stopFlywheel();
         shooter.stopShooterConveyor();
@@ -284,31 +389,6 @@ public class Superstructure extends SubsystemBase {
         intakeRoller.stop();
     }
 
-    private void updateAimingSetpoint() {
-        // Superstructure 可以更新底盘目标朝向，但不直接下发 swerve 模块或底盘速度命令。
-        if (limelight.hasTarget()) {
-            drive.setTargetHeading(drive.getRotation().plus(Rotation2d.fromDegrees(limelight.getTargetYaw())));
-        }
-    }
-
-    private void holdBackboardAtZero() {
-        shooter.setBackboardPosition(0.0);
-        if (shooter.isBackboardAtTarget()) {
-            shooter.backboardMotor.set(0.0);
-        } else {
-            shooter.outputBackboard();
-        }
-    }
-
-    private void holdBackboardForShot() {
-        shooter.setBackboardPosition(175.0 + backboardPositionOffset);
-        if (shooter.isBackboardAtTarget()) {
-            shooter.backboardMotor.set(0.0);
-        } else {
-            shooter.outputBackboard();
-        }
-    }
-
     private void stopAllMechanisms() {
         shooter.stopAll();
         intakeRoller.stop();
@@ -328,15 +408,19 @@ public class Superstructure extends SubsystemBase {
     }
 
     private void publishTelemetry() {
-        // 发布状态和门控信号，方便判断机器人为什么还没进入射击。
+        // 发布状态和各项门控信号，方便判断机器人为什么还没进入射击。
         table.getEntry("wantedState").setString(wantedState.name());
         table.getEntry("systemState").setString(systemState.name());
+        table.getEntry("timeInState").setDouble(timeInState());
         table.getEntry("canShoot").setBoolean(canShoot());
         table.getEntry("shooterAtSpeed").setBoolean(shooter.atSpeed());
         table.getEntry("driveAimed").setBoolean(drive.isAimed());
+        table.getEntry("driveHeadingError").setDouble(drive.getHeadingError());
+        table.getEntry("driveStationary").setBoolean(isDriveStationary());
         table.getEntry("intakeAtGoal").setBoolean(intakeRotater.atGoal());
+        table.getEntry("intakeBlockedByNotZeroed").setBoolean(intakeRotater.isBlockedByNotZeroed());
+        table.getEntry("hasNote").setBoolean(conveyor.hasNote());
         table.getEntry("flywheelSpeedOffsetRps").setDouble(flywheelSpeedOffsetRps);
         table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
     }
 }
-