gpt的超级状态机

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

@@ -1,24 +1,41 @@
 package frc.robot;
 
-import javax.net.ssl.TrustManager;
-
 import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 public final class Constants {
+    private Constants() {}
+
     public static final boolean LEDUsing = false;
-    public enum RobotStatus{
+
+    public enum RobotStatus {
         Stopped,
         AllTelop,
         PassingBall,
         AutoAimming,
-        Climbing,
         CrossingBump,
         CrossingTrench
     }
-    public class Shooter {
+
+    public static class Shooter {
+        // 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。
+        public static final int leftFlywheelID = 41;
+        public static final int rightFlywheelID = 42;
+        public static final int leftConveyorID = 43;
+        public static final int rightConveyorID = 45;
+        public static final int backboardMotorID = 44;
+
+        public static final double flywheelVoltsPerRps = 0.12;
+        public static final double flywheelIdleVoltage = 0.0;
+        public static final double shooterConveyorVoltage = 6.0;
+        public static final double shooterConveyorReverseVoltage = -4.0;
+        public static final double flywheelSpeedToleranceRps = 5.0;
+        public static final double defaultFlywheelTargetRps = 64.0;
+
         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)
             .withKV(0.099).withKA(0.0).withKS(0.0);
@@ -31,18 +48,18 @@ public final class Constants {
             .withKP(0.01).withKI(0).withKD(0)
             .withKV(0.03).withKA(0.0).withKS(0.010);
 
-        public class backboardPositionPID {
+        public static class backboardPositionPID {
             public static final double kP = 0.3;
             public static final double kI = 0.0;
             public static final double kD = 0.0;
+        }
 
+        public static final double backboardSpeedMax = 60;
+        public static final double backboardUpLimit = 3000.0;
+        public static final double backboardDownLimit = 0.0;
     }
-        public static final double backboardSpeedMax = 60; //也就是output
-        // 背板的上下限位角度
-        public static final double backboardUpLimit = 3000.0;  // 背板上限角度（根据实际需求设置）
-        public static final double backboardDownLimit = 0.0;  // 背板下限角度（根据实际需求设置）
-    }
-    public class AutoPositioning {
+
+    public static class AutoPositioning {
         public static final double autoRotationkP = 0.08;
         public static final double autoRotationForBumpTargetDegrees = 45;
         public static final double autoRotationForTrenchTargetDegrees = 0;
@@ -50,21 +67,20 @@ public final class Constants {
         public static final double teleopOffsetkP = 15;
 
         public static final double autoPositioningkP = 1.0;
-        public static final double autoPositioningAngleError = 1.0; // 角度误差容忍度，单位为度
+        public static final double autoPositioningAngleError = 1.0;
 
-        public static final double[] bumpY = {2.51,5.556};
-        public static final double[] trenchY = {0.639,7.43};
+        public static final double[] bumpY = {2.51, 5.556};
+        public static final double[] trenchY = {0.639, 7.43};
 
         public static final double TrenchtargetAngle = 0;
         public static final double TurningkP = 1.5;
     }
 
-    public class StartingPoints{
-        public class Red{
-            public static final Pose2d Point1 = new Pose2d(12.93, 7.47, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(0));
-            public static final Pose2d Point2 = new Pose2d(14.314, 4.15, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(-180));
+    public static class StartingPoints {
+        public static class Red {
+            public static final Pose2d Point1 = new Pose2d(12.93, 7.47, Rotation2d.fromDegrees(0));
+            public static final Pose2d Point2 = new Pose2d(14.314, 4.15, Rotation2d.fromDegrees(-180));
         }
-        
     }
 
     public enum DriveMode {
@@ -73,41 +89,19 @@ public final class Constants {
         SYSID,
         OTHER
     }
-    public class Pigeon {
+
+    public static class Pigeon {
         public static final int CANID = 0;
     }
 
-    public class Limelight {
+    public static 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 = true; // 是否使用 MetaTag2（如果你们的 pipeline 配置了这个功能的话）
-        public static final int climbPipelineIndex = 8;
+        public static final Boolean UsingMetaTag2 = true;
         public static final int locatePipelineIndex = 0;
-
-        // 误差死区 / 对齐判定
-        public static final double AutoClimbToleranceTX = 1.5;   // deg
-        public static final double AutoClimbToleranceTZ = 0.10;  // m
-        public static final double AutoClimbToleranceRY = 2.0;   // deg
-
-        // P 控制系数
-        public static final double AutoClimbKpTX = 0.020;   // deg -> rad/s
-        public static final double AutoClimbKpTZ = 0.90;    // m -> m/s
-        public static final double AutoClimbKpRY = 0.015;   // deg -> rad/s
-
-        // 最小输出（防止快到目标时推不动）
-        public static final double AutoClimbMinVX = 0.18;      // m/s
-        public static final double AutoClimbMinOmega = 0.10;   // rad/s
-
-        // 最大输出（防止冲太猛）
-        public static final double AutoClimbMaxVX = 1.20;      // m/s
-        public static final double AutoClimbMaxOmega = 0.60;   // rad/s
-
-        // 远距离 / 大角度时额外限速，可后续微调
-        public static final double AutoClimbFastTurnThresholdTX = 15.0; // deg
-        public static final double AutoClimbFastTurnOmega = 0.45;       // rad/s
     }
 
-    public class Field {
+    public static class Field {
         public static final double RedHubPositionX = 11.914;
         public static final double RedHubPositionY = 4.034;
         public static final double BlueHubPositionX = 4.625;
@@ -118,55 +112,54 @@ public final class Constants {
         public static final double BluePassingBallPosX = 3.184;
     }
 
+    public static class Intaker {
+        // 进球机构 CAN ID 同样是占位值，真实 ID 确认后集中在这里修改。
+        public static final int leftIntakeRollerID = 47;
+        public static final int rightIntakeRollerID = 46;
+        public static final int leftIntakeRotaterID = 48;
+        public static final int rightIntakeRotaterID = 49;
 
-    public class Intaker {
+        public static final double intakeRollerVoltage = 6.0;
+        public static final double outtakeRollerVoltage = -5.0;
+        public static final double holdRollerVoltage = 1.0;
 
-    public static final double intakeRotaterUpLimit = 0.0;  // rotator上限角度
-    public static final double intakeRotaterDownLimit = -7.6;  // rotator下限角度
-    public static final double intakeRotaterVelocity = 5; // rotater的速度（如果你用速度控制的话）
-    public static final double IntakeRotaterGravityFF = 0.0; // 重力前馈增益（需要实测调整）
+        public static final double intakeRotaterStowPosition = 0.0;
+        public static final double intakeRotaterGroundPosition = -7.6;
+        public static final double intakeRotaterHandoffPosition = -2.5;
+        public static final double intakeRotaterSafePosition = 0.0;
+        public static final double intakeRotaterManualVoltage = 2.0;
+
+        public static final double intakeRotaterUpLimit = 0.0;
+        public static final double intakeRotaterDownLimit = -7.6;
+        public static final double intakeRotaterVelocity = 5;
+        public static final double IntakeRotaterGravityFF = 0.0;
 
         public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs()
             .withKP(0.00).withKI(0.00).withKD(0.00)
             .withKV(0.00).withKA(0.00).withKS(0.00);
-    // getter 如果你只用固定速度开关，也可以先不启闭环；若用闭环速度，这里 kP/kV/KS 等需要实测调参(这句fromAI)
 
         public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs()
             .withKP(0.00).withKI(0.00).withKD(0.00)
             .withKV(0.00).withKA(0.00).withKS(0.00);
-    //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
 
         public static final double IntakeGetterSpeedforAuto = 0.5;
-    // 单位RPS
         public static final double positionToleranceRotations = 0.00;
-    //rotater容差
-
     }
-    public static class Climber {
 
-    public static final int motorID = 49;
+    public static class Conveyor {
+        public static final int mainConveyorID = 50;
+        public static final double feedVoltage = 6.0;
+        public static final double reverseVoltage = -5.0;
+        public static final double holdVoltage = 0.8;
+    }
 
-    public static final boolean inverted = false;
-
-    public static double climbUpTicks = 94; // TODO: 实测后调整
-    public static double climbDownTicks = 0; // TODO: 实测后调整
-    public static double climbDownAutoTicks = 0; // TODO: 实测后调整，自动爬时可能不需要完全降到底
-    public static final double climbVelocity = -200;
-    public static final double holdToleranceRot = 0.5; 
-    public static final double deltaDownTicks = 1;
-
-    public static final Slot0Configs slot0Configs = new Slot0Configs();
-
-    static {
-        // TODO: PID/F 值你们实测后填
-        slot0Configs.kP = 0.2;
-        slot0Configs.kI = 0.0;
-        slot0Configs.kD = 0.0;
-
-        // TODO: 如果要前馈（速度闭环一般建议有 kV）
-        slot0Configs.kS = 0.0;
-        slot0Configs.kV = 0.1;
-        slot0Configs.kA = 0.0;
+    public static class Superstructure {
+        // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
+        public static final double shooterFeedPercent = 1.0;
+        public static final double shooterReversePercent = -0.6;
+        public static final double cleanupSeconds = 0.3;
+        public static final double shootTimeoutSeconds = 1.0;
+        public static final double aimToleranceDegrees = 1.5;
+        public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
     }
 }
-}

src/main/java/frc/robot/Main.java

@@ -1,6 +1,6 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
+// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
+// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
+// 修改和/或分享本代码。
 
 package frc.robot;
 

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

@@ -1,6 +1,6 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
+// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
+// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
+// 修改和/或分享本代码。
 
 package frc.robot;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -30,9 +30,13 @@ public class Robot extends TimedRobot {
     @Override
     public void disabledInit() {
         System.out.println("Robot disabledInit called");
+        // 进入 disabled 时强制所有重构后的机构回到安全状态，
+        // 即使上一周期还有手动或自动请求也不能继续动作。
+        m_robotContainer.superstructure.requestIdle();
         m_robotContainer.shooterSubsystem.stopAll();
-        // m_robotContainer.climberSubsystem.stopAll();
-        m_robotContainer.IntakerSubsystem.stopAll();
+        m_robotContainer.intakeRollerSubsystem.stop();
+        m_robotContainer.intakeRotaterSubsystem.stop();
+        m_robotContainer.conveyorSubsystem.stop();
     }
 
     @Override

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

@@ -9,35 +9,35 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.RobotStatus;
+import frc.robot.controls.Autocommand;
 import frc.robot.controls.DriveControls;
-import frc.robot.controls.IntakerControls;
-import frc.robot.controls.ShooterControls;
+import frc.robot.controls.OperatorControls;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
-import frc.robot.subsystems.IntakerSubsystem;
+import frc.robot.subsystems.ConveyorSubsystem;
+import frc.robot.subsystems.IntakeRollerSubsystem;
+import frc.robot.subsystems.IntakeRotaterSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.LimelightSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.Superstructure;
 import frc.robot.tuning.ConfigTalonFXMotorTuner;
 import frc.robot.tuning.ConfigTalonFXSMotorTuner;
 import frc.robot.tuning.DriveGainsTuner;
-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;
+import frc.robot.utils.LedBindings;
+
 public class RobotContainer {
 
   /* ====================== */
   /*         手柄            */
   /* ====================== */
 
-  // 使用2个 Xbox 手柄（端口 0,1）
+  // 使用两个 Xbox 手柄：0 号给底盘驾驶，1 号给机构操作员。
   private final CommandXboxController controllerlower = new CommandXboxController(0);
   private final CommandXboxController controllerupper = new CommandXboxController(1);
 
@@ -45,60 +45,61 @@ public class RobotContainer {
   /*         子系统           */
   /* ====================== */
 
-  // Phoenix TunerX 生成的底盘
+  // 这里继续使用 Phoenix TunerX 生成的底盘，避免改动已经验证过的 swerve 底层。
   public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
-  //public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
   public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
-  public final IntakerSubsystem IntakerSubsystem = new IntakerSubsystem();
+  public final IntakeRollerSubsystem intakeRollerSubsystem = new IntakeRollerSubsystem();
+  public final IntakeRotaterSubsystem intakeRotaterSubsystem = new IntakeRotaterSubsystem();
+  public final ConveyorSubsystem conveyorSubsystem = new ConveyorSubsystem();
+  public final LimelightSubsystem limelightSubsystem = new LimelightSubsystem();
 
-  // LED 子系统
-  private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72         
+  // Superstructure 是所有得分机构的唯一调度器。
+  // 控制层和自动命令只能向它请求状态，不能直接命令电机。
+  public final Superstructure superstructure = new Superstructure(
+      shooterSubsystem,
+      intakeRollerSubsystem,
+      intakeRotaterSubsystem,
+      conveyorSubsystem,
+      drivetrain,
+      limelightSubsystem
+  );
+
+  // LED 子系统仍然只负责灯带显示，不参与机构状态判断。
+  private final LEDSubsystem leds = new LEDSubsystem(0, 72, 0); // 总长度 = 72 + 72
 
   private final RobotStatusManager robotStatusManager = new RobotStatusManager();
+
   /* ====================== */
-  /*     控制封装/日志        */
+  /*     控制封装/调参        */
   /* ====================== */
 
-  public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
-  private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
-  //private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
-  private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
+  public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager);
+
+  // 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
+  private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
   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);
+  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);
 
   private final SendableChooser<Command> autoChooser;
-  
   private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
 
-
-  
   public RobotContainer() {
-
     Pose2d startingPose = Constants.StartingPoints.Red.Point2;
     drivetrain.resetPose(startingPose);
     configueSwerve();
     configueShooter();
-    // configueClimber();
     bingdingLED();
     configueIntaker();
-    // Autocommand.preNameCommands(
-    //     climberSubsystem,
-    //     IntakerSubsystem,
-    //     drivetrain,
-    //     shooterSubsystem,
-    //     driveControls,
-    //     shooterControls
-    // );
-
     Autocommand.preNameCommands(
-        IntakerSubsystem,
-        drivetrain,
-        shooterSubsystem,
+        superstructure,
         driveControls,
-        shooterControls
+        operatorControls
     );
     SignalLogger.enableAutoLogging(false);
     autoChooser = AutoBuilder.buildAutoChooser();
@@ -114,52 +115,51 @@ public class RobotContainer {
     Pose2d startingPose = Constants.StartingPoints.Red.Point2;
     drivetrain.resetPose(startingPose);
 
-    // 默认驾驶命令 
-    /*
-     * LeftX 底盘左右
-     * LeftY 底盘前后  Bump/Trench偏移未完成
-     * RightX Aiming偏移    AllTelop模式下 底盘旋转
-     * 
-    */
+    // 默认驾驶命令说明：
+    // 左摇杆 X 控制底盘左右平移。
+    // 左摇杆 Y 控制底盘前后平移，越障/壕沟偏移逻辑目前仍沿用旧状态。
+    // 右摇杆 X 在自动瞄准模式下作为偏移量，在全手动模式下控制底盘旋转。
     drivetrain.setDefaultCommand(driveControls.defaultDriveCommand());
 
-    // Disabled 时进入 idle（防止模块乱动）
+    // 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。
     RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
 
-    
-    //左保险：按住刹车
+    // 左保险：按住刹车。
     controllerlower.leftBumper()
         .onTrue(Commands.runOnce(driveControls::emergencyStop))
         .whileTrue(driveControls.brakeWhileHeld());
 
-    // 右保险：按住加速（Boost）
+    // 右保险：按住开启加速模式，松开后恢复普通速度。
     controllerlower.rightBumper()
         .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
         .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
 
-    // Back：重置场地坐标系角度
+    // Back 键：重置场地坐标系角度。
     controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
 
-    // Start：强行使用metaTag2全场定位
+    // Start 键：强行使用 shooter 侧 Limelight 的 MegaTag2 全场定位。
     controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
 
-    controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+    controllerlower.a().whileTrue(drivetrain.run(() -> {
+      drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
       System.out.println("Try using shooter for metatag2");
     }));
-    controllerlower.b().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
+    controllerlower.b().whileTrue(drivetrain.run(() -> {
+      drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
       System.out.println("Try using intaker for metatag2");
     }));
-    //LM 按下时autoaimming，松开时alltelop
+
+    // 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
     // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
     // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
 
-    //双操均可触发autoaimming模式，松开时alltelop
-    Trigger autoAim = controllerupper.rightBumper();//.or(controllerlower.rightStick());
+    // 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。
+    Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。
     autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
     autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
 
-    //双操均可触发passingball模式，松开时alltelop
-    Trigger autoAim2 = controllerupper.leftBumper();//.or(controllerlower.leftStick());
+    // 上操作员左保险：按住进入传球模式，松开回到全手动模式。
+    Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。
     autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
     autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
 
@@ -170,21 +170,26 @@ public class RobotContainer {
     controllerlower.rightTrigger()
         .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
         .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
-
-    controllerlower.a()
-    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
-    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
   }
 
-  private void configueShooter(){
-    shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
-    controllerupper.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
-    controllerupper.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
-    controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
-    controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
-    controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
-    controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
-    
+  private void configueShooter() {
+    // 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure
+    // 根据飞轮速度、底盘瞄准和进球旋转机构位置统一判断。
+    controllerupper.rightTrigger()
+        .onTrue(operatorControls.requestShootCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+    controllerupper.y()
+        .onTrue(operatorControls.requestPrepShootCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+    controllerupper.b()
+        .onTrue(operatorControls.requestEjectCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+    controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(-1)));
+    controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(1)));
+    controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
+    controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
+    controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets()));
+    controllerupper.start().onTrue(operatorControls.resetBackboardCommand());
   }
 
   private void bingdingLED() {
@@ -192,7 +197,6 @@ public class RobotContainer {
   }
 
   public Command getAutonomousCommand() {
-
     return autoChooser.getSelected();
   }
 
@@ -208,17 +212,12 @@ public class RobotContainer {
     gameData2026.periodic();
   }
 
-  // private void configueClimber() {
-  //   climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
-  // }
-
   private void configueIntaker() {
-    IntakerSubsystem.setDefaultCommand(intakerControls.defaultIntakerCommand());
-    //controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
+    // 进球相关按钮也只请求 Superstructure 状态。
+    // A 键是校准动作，只应在进球机构处于已知零点姿态时按下。
+    controllerupper.leftTrigger()
+        .onTrue(operatorControls.requestIntakeCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+    controllerupper.a().onTrue(operatorControls.resetIntakeRotaterEncoderCommand(intakeRotaterSubsystem));
   }
-
-  // public ClimberSubsystem getClimberSubsystem() {
-  //     return climberSubsystem;
-  // }
-
 }

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

@@ -2,188 +2,69 @@ package frc.robot.controls;
 
 import com.pathplanner.lib.auto.NamedCommands;
 
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.Subsystem;
-import frc.robot.subsystems.ClimberSubsystem;
-import frc.robot.RobotStatusManager;
-import frc.robot.subsystems.IntakerSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.CommandSwerveDrivetrain;
-import frc.robot.Constants;
-import frc.robot.Constants.RobotStatus;
-
-    public class Autocommand extends SequentialCommandGroup{
+import frc.robot.subsystems.Superstructure;
 
+/**
+ * 注册 PathPlanner 的 NamedCommands。
+ *
+ * 自动路径只请求机器人级动作，不直接设置 shooter、intake 或 conveyor 电机；
+ * 这样自动和手动都会走同一套 Superstructure 状态机。
+ */
+public class Autocommand extends SequentialCommandGroup {
     public static void preNameCommands(
-            // ClimberSubsystem climber,
-            IntakerSubsystem intaker,
-            CommandSwerveDrivetrain drivetrain,
-            ShooterSubsystem shooter,
+        Superstructure superstructure,
         DriveControls driveControls,
-            ShooterControls shooterControls
+        OperatorControls operatorControls
     ) {
-        // NamedCommands.registerCommand("climb_up", climb_up(climber));
-        // NamedCommands.registerCommand("climb_down", climb_down(climber));
-        NamedCommands.registerCommand("start_intake", start_intake(intaker));
-        NamedCommands.registerCommand("start_intake_and_conveyor", start_intake_and_conveyor(intaker,shooter));
-        NamedCommands.registerCommand("stop_intake_and_conveyor", stop_intake_and_conveyor(intaker,shooter));
-        NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker));
-        NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
-        NamedCommands.registerCommand("intaker_down", intaker_down(intaker));
-        NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
-        //NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
-        NamedCommands.registerCommand("shoot", shoot(shooter));
+        NamedCommands.registerCommand("start_intake", startIntake(superstructure));
+        NamedCommands.registerCommand("start_intake_and_conveyor", startIntake(superstructure));
+        NamedCommands.registerCommand("stop_intake_and_conveyor", idle(superstructure));
+        NamedCommands.registerCommand("start_intake_timelimit", timedIntake(superstructure));
+        NamedCommands.registerCommand("stop_intake", idle(superstructure));
+        NamedCommands.registerCommand("intaker_down", startIntake(superstructure));
+        NamedCommands.registerCommand("intaker_up", idle(superstructure));
+        NamedCommands.registerCommand("shoot", shoot(superstructure));
         NamedCommands.registerCommand("Shake", shake(driveControls));
         NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
-        NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls));
-        // NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber));
-        NamedCommands.registerCommand("reset_backboard0", resetBackboard0Command(shooterControls));
-
+        NamedCommands.registerCommand("AutoShoot", operatorControls.autoShootToHubCommand());
+        NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command());
     }
 
-        public static Command climb_up(ClimberSubsystem climber) {
-            return new RunCommand(() -> {
-                climber.climbUp();  // 控制爬升器上升
-            }, climber).withTimeout(2).andThen(new RunCommand(() -> {
-                climber.climbUpNonStopforAuto();
-            }, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
-                climber.captureHoldPosition();  // 捕获当前爬升器位置以保持
-                climber.holdPosition();  // 停止并锁定当前爬升器位置
-            }, climber));
-        }
-        public static Command climb_down(ClimberSubsystem climber) {
-            return new RunCommand(() -> {
-                climber.climbDown();  // 控制爬升器下降
-            }, climber).withTimeout(2.0)
-            .andThen(new InstantCommand(() -> {
-                climber.captureHoldPosition();  // 捕获当前爬升器位置以保持
-                climber.holdPosition();         // 停止并锁定当前爬升器位置
-            }, climber));
+    public static Command startIntake(Superstructure superstructure) {
+        return superstructure.requestIntakeCommand();
     }
 
-        public static Command start_intake(IntakerSubsystem intaker) {
-            return new InstantCommand(() -> {
-                intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto);  // 启动吸球
-            }, intaker);
+    public static Command timedIntake(Superstructure superstructure) {
+        return Commands.sequence(
+            superstructure.requestIntakeCommand(),
+            Commands.waitSeconds(1.5),
+            superstructure.requestIdleCommand()
+        );
     }
-        public static Command start_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
-            return new InstantCommand(() -> {
-                shooter.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeedForAuto);
-                intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto);  // 启动吸球
 
-            }, intaker, shooter);
+    public static Command idle(Superstructure superstructure) {
+        return superstructure.requestIdleCommand();
     }
-        public static Command stop_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
-            return new InstantCommand(() -> {
-                intaker.setIntakerGetterSpeed(0);  // 启动吸球
-                shooter.setConveyorSpeedByRPS(0);
-            }, intaker, shooter);
-        }
-        public static Command start_intake_timelimit(IntakerSubsystem intaker) {
-            return new RunCommand(() -> {
-                intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto);  // 吸球机构下降
-            }, intaker).withTimeout(1.5).andThen(new InstantCommand(() -> {
-                intaker.setIntakerGetterSpeed(0);  // 停止旋转
-            }, intaker));
-        }
-        public static Command stop_intake(IntakerSubsystem intaker) {
-            return new RunCommand(() -> {
-                intaker.setIntakerGetterSpeed(0);  // 停止吸球
-            }, intaker);
-        }
-        public static Command intaker_down(IntakerSubsystem intaker) {
-            return new RunCommand(() -> {
-                intaker.IntakerDownNonStop();   // 吸球机构下降
-            }, intaker).withTimeout(0.6)
 
-            .andThen(new RunCommand(() -> {
-                intaker.IntakerUpNonStop();     // 吸球机构上升
-            }, intaker).withTimeout(0.1))
+    // 自动射击和手动射击使用同一套 gated shooting 流程。
+    public static Command shoot(Superstructure superstructure) {
+        return Commands.sequence(
+            superstructure.requestPrepShootCommand(),
+            Commands.waitUntil(superstructure::canShoot).withTimeout(1.5),
+            superstructure.requestShootCommand(),
+            Commands.waitSeconds(1.2),
+            superstructure.requestIdleCommand()
+        );
+    }
 
-            .andThen(new InstantCommand(() -> {
-                intaker.stopIntakerotater();    // 停止
-            }, intaker));
-        }
-        public static Command intaker_up(IntakerSubsystem intaker) {
-            return new RunCommand(() -> {
-                intaker.IntakerUpNonStop();  // 吸球机构上升
-            }, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> {
-                intaker.stopIntakerotater();  // 停止旋转
-            }, intaker));
-        }
-        // public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
-        // return Commands.sequence(
-        //     new InstantCommand(() -> {
-        //         LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
-        //     }),
-        //     Commands.runEnd(
-        //         () -> {
-        //             drivetrain.driveToAprilTag();
-        //         },
-        //         () -> {
-        //             LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
-        //         },
-        //         drivetrain
-        //     ).until(() -> drivetrain.isAlignedToAprilTag()));
-        // }
-
-        public static Command shoot(ShooterSubsystem shooter) {
-            return new RunCommand(() -> {
-                shooter.setFlywheelSpeedByRPS(64);
-                shooter.setBackboardPosition(175);  // 设置自动射击转速
-                if(shooter.isBackboardAtTarget()){
-                        shooter.backboardMotor.set(0);
-                    }else{
-                        shooter.outputBackboard();
-                    }  
-            }, shooter).withTimeout(0.25).andThen(new RunCommand(() -> {
-                shooter.setFlywheelSpeedByRPS(64);
-                shooter.setBackboardPosition(175);
-                if(shooter.isBackboardAtTarget()){
-                        shooter.backboardMotor.set(0);
-                    }else{
-                        shooter.outputBackboard();
-                    }  
-                shooter.setConveyorSpeedByRPS(25);
-            }, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> {
-                shooter.setFlywheelSpeedByRPS(0);  // 停止射击
-                //shooter.setBackboardPosition(0);  // 恢复背板位置
-                shooter.setConveyorSpeedByRPS(0);  // 停止输送
-            }, shooter)));
-        }
     public static Command autoAim(DriveControls driveControls) {
         return driveControls.autoAimCommand();
     }
 
-        public static Command autoShoot(ShooterControls shooterControl) {
-            return shooterControl.autoShootToHubCommand();
-        }
-        public static Command resetBackboard0Command(ShooterControls shooterControl) {
-            return shooterControl.resetBackboard0Command();
-        }
-
     public static Command shake(DriveControls driveControls) {
         return driveControls.shakeCommand();
     }
-
-        public static Command climber_Zeroencoder(ClimberSubsystem climber) {
-            return climber.initCommand();
-        }
-
-
-
-    }
-
-
-
-
-
-
+}

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

@@ -1,57 +0,0 @@
-package frc.robot.controls;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants;
-import frc.robot.subsystems.ClimberSubsystem;
-
-
-public class ClimberControls {
-
-    private final ClimberSubsystem climber;
-    private final CommandXboxController controller;
-
-    private final NetworkTable table;
-
-    public ClimberControls(ClimberSubsystem climber, CommandXboxController controller) {
-        this.climber = climber;
-        this.controller = controller;
-        this.table = NetworkTableInstance.getDefault().getTable("ClimberControls");
-    }
-
-    public Command defaultClimberCommand() {
-        return Commands.run(()->{
-            //climber.climbUp();
-                if(controller.getRightY() < -0.5){
-                    climber.climbUpNonStop();
-                } else if(controller.getRightY() > 0.5){
-                    climber.climbDownNonStop();
-                } else if (controller.y().getAsBoolean()) {
-                    climber.climbUp();
-                } else if (controller.x().getAsBoolean()) {
-                    climber.climbDown();
-                } else {
-                    // 松开：记录当前位置 + 锁死
-                    climber.captureHoldPosition();
-                    climber.holdPosition();
-                }
-                
-
-                // 调试用
-                table.getEntry("xHeld_up").setBoolean(controller.x().getAsBoolean());
-                table.getEntry("yHeld_down").setBoolean(controller.y().getAsBoolean());
-        }, climber);
-
-    }
-
-    public Command resetClimberEncoderCommand(ClimberSubsystem climber) {
-        return Commands.runOnce(climber::zeroEncoder, climber);
-    }
-    
-}

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

@@ -151,9 +151,6 @@ public class DriveControls {
             case AllTelop:
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
-            case Climbing:
-                // alignToClimb();
-                break;
             case CrossingBump:
                 double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
                 if (Math.abs(differenceBump) < 0.1) {
@@ -405,16 +402,6 @@ public class DriveControls {
     public Command getAllianceColorCommand() {
         return Commands.runOnce(this::setTeamColors);
     }
-    // public void alignToClimb(){
-    //     LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
-    //     if(!drivetrain.isAlignedToAprilTag()){
-    //         drivetrain.driveToAprilTag();
-    //     }
-    //     else{
-    //         LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
-    //         drivetrain.stop();
-    //     }
-    // }
     public Command autoAimCommand() {
         return drivetrain.applyRequest(() -> {
         double[] distanceAndRotation = calculateDistanceAndRotationToHub();

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

@@ -1,96 +0,0 @@
-package frc.robot.controls;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.subsystems.IntakerSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.Constants;
-
-
-public class IntakerControls {
-    private final IntakerSubsystem IntakerSubsystem;
-    private final CommandXboxController controller;
-    private final ShooterSubsystem ShooterSubsystem;
-
-    //private static final double ROTATER_STEP_PER_CYCLE = 0.00;
-
-    public IntakerControls(IntakerSubsystem Intaker, ShooterSubsystem Shooter, CommandXboxController controller) {
-        this.ShooterSubsystem = Shooter;
-        this.IntakerSubsystem = Intaker;
-        this.controller = controller;
-    }
-
-    
-
-        //B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转
-        // controller.a()
-        //     .whileTrue(
-        //         Commands.run(() -> {
-        //             // rotater下降：范围会被 subsystem 限制
-        //             double cur = intakerSubsystem.getIntakerotaterPosition();
-        //             intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
-
-        //             // getter 开
-        //             intakerSubsystem.setIntakeGetterOn(true);
-        //         }, intakerSubsystem)
-        //     )
-        //     .onFalse(
-        //         Commands.runOnce(() -> {
-        //             // 松：把当前位置作为target保持
-        //             double cur = intakerSubsystem.getIntakerotaterPosition();
-        //             intakerSubsystem.setIntakerRotaterPosition(cur);
-
-        //             // gtter继续转
-        //             intakerSubsystem.setIntakeGetterOn(true);
-        //         }, intakerSubsystem)
-        //     );
-        // // === B.B：按住上升+getter停；松开后rotater保持当前位置
-        // controller.b()
-        //     .whileTrue(
-        //         Commands.run(() -> {
-        //             // rotater升
-        //             double cur = intakerSubsystem.getIntakerotaterPosition();
-        //             intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
-
-        //             // getter关
-        //             intakerSubsystem.setIntakeGetterOn(false);
-        //         }, intakerSubsystem)
-        //     )
-        //     .onFalse(
-        //         Commands.runOnce(() -> {
-        //             // 松：保持当前位置
-        //             double cur = intakerSubsystem.getIntakerotaterPosition();
-        //             intakerSubsystem.setIntakerRotaterPosition(cur);
-
-        //             // getter仍然停止
-        //             intakerSubsystem.setIntakeGetterOn(false);
-        //         }, intakerSubsystem)
-        //     );
-    
-    public Command defaultIntakerCommand() {
-        return Commands.run(() -> {
-        double getterspeed = 0;
-        double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
-        
-        if (controller.getLeftTriggerAxis() > 0.1) {
-            getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3;
-            }
-
-        IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
-
-        if(controller.getLeftY() < -0.5){
-                    IntakerSubsystem.IntakerUpNonStop();
-            } else if(controller.getLeftY() > 0.5){
-                    IntakerSubsystem.IntakerDownNonStop();
-            } else {
-                    IntakerSubsystem.stopIntakerotater();
-            }
-        }, IntakerSubsystem);
-    }
-    public Command resetIntakerRotaterEncoderCommand(IntakerSubsystem intakerSubsystem) {
-        return Commands.runOnce(intakerSubsystem::zeroEncoder, intakerSubsystem);
-    }
-}

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

@@ -0,0 +1,93 @@
+package frc.robot.controls;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.IntakeRotaterSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.Superstructure;
+
+/**
+ * 上操作员手柄的“意图层”。
+ *
+ * 这里刻意不直接设置任何机构电机。按钮、自动辅助命令和调参快捷键都会
+ * 转换成 Superstructure 请求，由 Superstructure 统一处理多机构动作顺序。
+ */
+public class OperatorControls {
+    private final Superstructure superstructure;
+    private final ShooterSubsystem shooterSubsystem;
+
+    public OperatorControls(Superstructure superstructure, ShooterSubsystem shooterSubsystem) {
+        this.superstructure = superstructure;
+        this.shooterSubsystem = shooterSubsystem;
+    }
+
+    public Command requestIdleCommand() {
+        return superstructure.requestIdleCommand();
+    }
+
+    public Command requestIntakeCommand() {
+        return superstructure.requestIntakeCommand();
+    }
+
+    public Command requestPrepShootCommand() {
+        return superstructure.requestPrepShootCommand();
+    }
+
+    public Command requestShootCommand() {
+        return superstructure.requestShootCommand();
+    }
+
+    public Command requestEjectCommand() {
+        return superstructure.requestEjectCommand();
+    }
+
+    public void adjustFlywheelSpeedOffset(double offset) {
+        superstructure.adjustFlywheelSpeedOffset(offset);
+    }
+
+    public void adjustBackboardRateOffset(double offset) {
+        superstructure.adjustBackboardPositionOffset(offset);
+    }
+
+    public void resetOffsets() {
+        superstructure.resetOffsets();
+    }
+
+    public Command resetBackboardCommand() {
+        return Commands.runOnce(superstructure::resetBackboardEncoder, shooterSubsystem);
+    }
+
+    public Command resetIntakeRotaterEncoderCommand(IntakeRotaterSubsystem intakeRotater) {
+        return Commands.runOnce(intakeRotater::zeroPosition, intakeRotater);
+    }
+
+    // PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球，
+    // 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。
+    public Command autoShootToHubCommand() {
+        return Commands.sequence(
+            superstructure.requestPrepShootCommand(),
+            Commands.waitUntil(superstructure::canShoot).withTimeout(1.5),
+            superstructure.requestShootCommand(),
+            Commands.waitSeconds(1.2),
+            superstructure.requestIdleCommand()
+        );
+    }
+
+    // 保留旧背板归零/回零命令，方便调参和现场恢复。
+    public Command resetBackboard0Command() {
+        return Commands.sequence(
+            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/controls/ShooterControls.java

@@ -1,225 +0,0 @@
-package frc.robot.controls;
-
-import java.util.Set;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants;
-import frc.robot.RobotStatusManager;
-import frc.robot.Constants.Intaker;
-import frc.robot.Constants.RobotStatus;
-import frc.robot.subsystems.IntakerSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-
-public class ShooterControls {
-    private final ShooterSubsystem shooterSubsystem;
-    private final CommandXboxController controller;
-
-    private double flywheelSpeedOffset = 0.0;
-    private double backboardPositionOffset = 0.0;
-    private NetworkTable shooterControlTable;
-    private double conveyorSpeed = 0.0;
-    private double flywheelSpeed = 0;
-    private double backboardPosition = 0;
-    private final RobotStatusManager robotStatusManager;
-    public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) {
-        this.shooterSubsystem = shooterSubsystem;
-        this.controller = controller;
-        this.robotStatusManager = rsm;
-        shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl");
-    }
-    
-    public Command defaultShooterCommand() {
-        return Commands.run(() -> {
-            switch (robotStatusManager.getStatus()) {
-                case Stopped:
-                    shooterSubsystem.stopMotors();
-                    break;
-                case AllTelop:
-                    shooterSubsystem.setBackboardPosition(0); 
-                    if(shooterSubsystem.isBackboardAtTarget()){
-                        shooterSubsystem.backboardMotor.set(0);
-                    }else{
-                        shooterSubsystem.outputBackboard();
-                    }  
-                    shooterSubsystem.setFlywheelSpeedByRPS(0);
-                    if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
-                    else{shooterSubsystem.setConveyorSpeedByRPS(0);}
-                    break;
-                case PassingBall:
-                    var xP = shooterSubsystem.getDistanceToPassball();
-                    xP = Math.max(2.5, Math.min(10.0, xP));
-                    flywheelSpeed = +0.004960317456971805*xP*xP*xP*xP*xP*xP*xP-0.22361111097424535*xP*xP*xP*xP*xP*xP+4.234722219890569*xP*xP*xP*xP*xP-43.63194442298942*xP*xP*xP*xP+264.05555544032126*xP*xP*xP-938.6444440829168*xP*xP+1818.2047612908214*xP-1415.9999995643104;
-                    flywheelSpeed += flywheelSpeedOffset;
-                    backboardPosition = +0.10912698402591796*xP*xP*xP*xP*xP*xP*xP-5.277777773586415*xP*xP*xP*xP*xP*xP+105.55555548303796*xP*xP*xP*xP*xP-1125.6944437656143*xP*xP*xP*xP+6865.763885174154*xP*xP*xP-23769.027765888906*xP*xP+43328.57140795847*xP-31599.99998506075;
-                    conveyorSpeed = Constants.Shooter.conveyorSpeed;
-                    if (controller.getRightTriggerAxis() < 0.1) {
-                        flywheelSpeed = 0;
-                        conveyorSpeed = 0;
-                    }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
-                        conveyorSpeed = 0;
-                    }
-                    //if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
-                    shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
-                    shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
-                    shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
-                    shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
-                    shooterSubsystem.setBackboardPosition(backboardPosition);  
-                    if(shooterSubsystem.isBackboardAtTarget()){
-                        shooterSubsystem.backboardMotor.set(0);
-                    }else{
-                        shooterSubsystem.outputBackboard();
-                    }  
-                    break;
-                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 += 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 += backboardPositionOffset;
-                    conveyorSpeed = Constants.Shooter.conveyorSpeed;
-                    if (controller.getRightTriggerAxis() < 0.1) {
-                        flywheelSpeed = 0;
-                        conveyorSpeed = 0;
-                    }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
-                        conveyorSpeed = 0;
-                    }
-                    //if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
-                    shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
-                    shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
-                    shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
-                    shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
-                    shooterSubsystem.setBackboardPosition(backboardPosition);  
-                    if(shooterSubsystem.isBackboardAtTarget()){
-                        shooterSubsystem.backboardMotor.set(0);
-                    }else{
-                        shooterSubsystem.outputBackboard();
-                    }  
-                    break;
-                case Climbing:
-                case CrossingBump:
-                case CrossingTrench:
-                    if(shooterSubsystem.getBackboardPosition() <= 20)
-                        shooterSubsystem.stopMotors();
-                    else{
-                        shooterSubsystem.setBackboardPosition(0);    
-                        shooterSubsystem.outputBackboard();
-                    }
-                    break;
-                    
-            }
-        }, shooterSubsystem);
-    }
-
-    // public Command resetBackboardCommand(){
-    //     return 
-    //         Commands.runOnce(()->{
-    //             shooterSubsystem.backboardMotor.set(-0.1);
-    //             shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzing;}, shooterSubsystem)
-    //         .andThen(
-    //             Commands.waitSeconds(0.5))
-    //         .andThen(Commands.runOnce(()->{
-    //             shooterSubsystem.backboardMotor.set(0);
-    //             shooterSubsystem.backboardEncoder.reset();
-    //             shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem))
-    //         .andThen(
-    //             Commands.waitSeconds(1))
-    //         .andThen(Commands.runOnce(()->{
-    //             shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzed;}, shooterSubsystem));
-    // }
-
-    public void outputInfo(){
-        double currentConveyorSpeed = shooterSubsystem.conveyorMotor.getVelocity().getValueAsDouble();
-        double currentFlywheelSpeed = shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble();
-        double currentBackboardRate = shooterSubsystem.getBackboardPosition();
-        System.out.println("Conveyor: " + currentConveyorSpeed + "\t Flywheel: " + currentFlywheelSpeed + "\t Backboard:" + currentBackboardRate);
-    }
-    public Command resetBackboardCommand(){
-        return 
-            Commands.runOnce(()->{
-                shooterSubsystem.backboardEncoder.reset();
-                shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem);
-    }
-
-    public void adjustFlywheelSpeedOffset(double offset) {
-        flywheelSpeedOffset += offset;
-    }
-
-    public void adjustBackboardRateOffset(double offset) {
-        backboardPositionOffset += offset;
-    }
-    public void resetOffsets(){
-        flywheelSpeedOffset = 0.0;
-        backboardPositionOffset = 0.0;
-    }
-
-    public Command autoShootToHubCommand() {
-        return Commands.defer(() -> {
-            double x = shooterSubsystem.getDistanceToHub();
-            x = Math.max(1.2, Math.min(4.5, x));
-
-            double flywheelTargetSpeed = -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;
-            double backboardTargetPosition = +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;
-            double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed;
-
-            return new RunCommand(() -> {
-                shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
-                shooterSubsystem.setConveyorSpeedByRPS(0.0);
-                shooterSubsystem.setBackboardPosition(backboardTargetPosition);
-                if (shooterSubsystem.isBackboardAtTarget()) {
-                    shooterSubsystem.backboardMotor.set(0);
-                } else {
-                    shooterSubsystem.outputBackboard();
-                }
-            }, shooterSubsystem)
-            .until(() ->
-                shooterSubsystem.isBackboardAtTarget()
-                && shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
-            )
-            .withTimeout(0.3)
-            .andThen(new RunCommand(() -> {
-                shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
-                shooterSubsystem.setBackboardPosition(backboardTargetPosition);
-                if (shooterSubsystem.isBackboardAtTarget()) {
-                    shooterSubsystem.backboardMotor.set(0);
-                } else {
-                    shooterSubsystem.outputBackboard();
-                }
-                shooterSubsystem.setConveyorSpeedByRPS(25);
-            }, shooterSubsystem).withTimeout(5.0))
-            .andThen(new InstantCommand(() -> {
-                shooterSubsystem.setFlywheelSpeedByRPS(0.0);
-                shooterSubsystem.setConveyorSpeedByRPS(0.0);
-                shooterSubsystem.backboardMotor.set(0.0);
-            }, shooterSubsystem));
-        }, Set.of(shooterSubsystem));
-    }
-    
-    public Command resetBackboard0Command() {
-        return new RunCommand(() -> {
-            shooterSubsystem.setBackboardPosition(0);
-
-            if (shooterSubsystem.isBackboardAtTarget()) {
-                shooterSubsystem.backboardMotor.set(0);
-            } else {
-                shooterSubsystem.outputBackboard();
-            }
-
-        }, shooterSubsystem)
-        .until(() -> shooterSubsystem.isBackboardAtTarget())
-        .andThen(new InstantCommand(() -> {
-            shooterSubsystem.backboardMotor.set(0);
-        }));
-    }
-}
-

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

@@ -1,127 +0,0 @@
-package frc.robot.subsystems;
-
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-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;
-
-
-public class ClimberSubsystem extends SubsystemBase {
-
-    private boolean autoClimbUsedInAuto = false;
-
-    private final TalonFX climberMotor;
-    private double targetPositionTicks;
-
-    private final PositionVoltage positionHoldRequest =
-            new PositionVoltage(0).withSlot(0);
-
-    private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
-
-    private double holdPositionTicks = 0.0;
-
-    private final NetworkTable climberTable;
-    
-
-    public ClimberSubsystem() {
-        climberMotor = new TalonFX(Constants.Climber.motorID);
-
-        climberMotor.getConfigurator().apply(Constants.Climber.slot0Configs);
-
-        var cfg = new MotorOutputConfigs();
-        cfg.Inverted = Constants.Climber.inverted
-                ? InvertedValue.Clockwise_Positive
-                : InvertedValue.CounterClockwise_Positive;
-        climberMotor.getConfigurator().apply(cfg);
-
-        climberTable = NetworkTableInstance.getDefault().getTable("Climber");
-
-        holdPositionTicks = getPositionTicks();
-    }
-    public void initialize() {
-        climberMotor.set(-0.07);
-    }
-
-    public double getPositionTicks() {
-        return climberMotor.getPosition().getValueAsDouble();
-    }
-
-    public double getVelocityRps() {
-        return climberMotor.getVelocity().getValueAsDouble();
-    }
-
-    public void climbUp() {
-        targetPositionTicks = Constants.Climber.climbUpTicks;  
-        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
-    }
-
-    public void climbDown() {
-        targetPositionTicks = Constants.Climber.climbDownTicks;  
-        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
-    }
-    public void climbDownAuto() {
-        targetPositionTicks = Constants.Climber.climbDownAutoTicks;  
-        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
-    }
-
-    public void climbDownNonStop(){
-        climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
-    }
-    
-    public void climbUpNonStop(){
-        climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
-    }
-    public void climbUpNonStopforAuto(){
-        climberMotor.set(0.07);
-    }
-
-    public void captureHoldPosition() {
-        holdPositionTicks = getPositionTicks();
-    }
-
-    public void holdPosition() {
-        climberMotor.setControl(
-                positionHoldRequest.withPosition(holdPositionTicks)
-        );
-    }
-
-    public void stopMotor() {
-        climberMotor.set(0);
-    }
-    public void setMotor(double speed) {
-        climberMotor.set(speed);
-    }
-
-    public Command initCommand() {
-        return Commands.sequence(
-            Commands.run(() -> climberMotor.set(-0.07), this).withTimeout(0.5),
-            Commands.runOnce(this::zeroEncoder, this)
-        );
-    }
-
-    public void zeroEncoder() {
-        climberMotor.setPosition(0);
-        stopMotor();
-    }
-
-    public void stopAll() {
-        climberMotor.set(0);
-    }
-
-    @Override
-    public void periodic() {
-        climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
-        climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
-        climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
-    }
-}

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

@@ -1,4 +1,5 @@
 package frc.robot.subsystems;
+
 import java.util.Optional;
 import java.util.function.Supplier;
 
@@ -15,40 +16,39 @@ import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants;
 import frc.robot.Constants.DriveMode;
 import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
-import frc.robot.subsystems.LimelightHelpers.RawFiducial;
-import frc.robot.Constants;
-
-import edu.wpi.first.networktables.*;
-
 
 /**
- * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
- * Subsystem so it can easily be used in command-based projects.
+ * Phoenix 6 Tuner X 生成底盘的命令式封装。
  *
- * Generated by the 2026 Tuner X Swerve Project Generator
- * https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+ * 底层 swerve、模块控制和里程计保持原结构；本类只在外层补充
+ * Command-Based、PathPlanner、Limelight 融合和上层状态读取接口。
  */
 public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
 
-    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    /* 蓝方视角下，正前方为 0 度。 */
     private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
-    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    /* 红方视角下，正前方为 180 度。 */
     private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
-    /* Keep track if we've ever applied the operator perspective before or not */
+    /* 记录是否已经应用过操作员视角。 */
     private boolean m_hasAppliedOperatorPerspective = false;
-    private boolean isAlignedToAprilTag = false;
+
+    // 上层状态机射击门控使用的目标朝向；正常底盘控制仍由手动驾驶掌控。
+    private Rotation2d targetHeading = Rotation2d.kZero;
 
     private DriveMode m_driveMode = DriveMode.OTHER;
 
@@ -66,7 +66,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private Field2d shooter_ll_field;
     private Field2d intaker_ll_field;
 
-
     public CommandSwerveDrivetrain(
         SwerveDrivetrainConstants drivetrainConstants,
         SwerveModuleConstants<?, ?, ?>... modules
@@ -86,12 +85,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker");
 
         configueAutoBuilder();
-
     }
 
-
-    private void configueAutoBuilder(){
-        
+    private void configueAutoBuilder() {
         RobotConfig config;
         try {
             config = RobotConfig.fromGUISettings();
@@ -100,20 +96,20 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         }
 
         AutoBuilder.configure(
-            this::getPose,                  // 当前机器人 Pose
-            this::resetPose,                // 重置里程计
-            this::getRobotRelativeSpeeds,   // ROBOT RELATIVE speeds
-            (speeds, feedforwards) -> driveRobotRelative(speeds),      // 用 speeds 驱动机器人
+            this::getPose,                 // 当前机器人位姿。
+            this::resetPose,               // 重置里程计。
+            this::getRobotRelativeSpeeds,  // 机器人坐标系速度。
+            (speeds, feedforwards) -> driveRobotRelative(speeds), // 按目标速度驱动机器人。
             new PPHolonomicDriveController(
-                new PIDConstants(5.0, 0.0, 0.0),   // X/Y PID
-                new PIDConstants(5.0, 0.0, 0.0)    // Theta PID
+                new PIDConstants(5.0, 0.0, 0.0), // X/Y 位置 PID。
+                new PIDConstants(5.0, 0.0, 0.0)  // 朝向 PID。
             ),
             config,
             () -> false,
             this
         );
-
     }
+
     public Pose2d getPose() {
         return getState().Pose;
     }
@@ -122,11 +118,37 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         return getState().Speeds;
     }
 
+    public ChassisSpeeds getChassisSpeeds() {
+        // 给上层代码使用的速度别名；原机器人坐标系方法继续留给 PathPlanner。
+        return getRobotRelativeSpeeds();
+    }
+
+    public ChassisSpeeds getFieldRelativeSpeeds() {
+        return ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeSpeeds(), getRotation());
+    }
+
+    public Rotation2d getRotation() {
+        return getState().Pose.getRotation();
+    }
+
+    public void setTargetHeading(Rotation2d targetHeading) {
+        this.targetHeading = targetHeading;
+    }
+
+    public double getHeadingError() {
+        return targetHeading.minus(getRotation()).getDegrees();
+    }
+
+    public boolean isAimed() {
+        // 上层状态机只读取这个门控，不直接控制 swerve 模块。
+        return Math.abs(getHeadingError()) <= Constants.Superstructure.aimToleranceDegrees;
+    }
+
     public void driveRobotRelative(ChassisSpeeds speeds) {
         ChassisSpeeds scaled = new ChassisSpeeds(
-            speeds.vxMetersPerSecond,                 // X 不变
-            speeds.vyMetersPerSecond,                 // Y 不变
-            speeds.omegaRadiansPerSecond * 1.3        // 只放大旋转
+            speeds.vxMetersPerSecond,          // X 方向速度保持原值。
+            speeds.vyMetersPerSecond,          // Y 方向速度保持原值。
+            speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转输出，沿用原驾驶手感。
         );
 
         setControl(
@@ -140,14 +162,26 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         );
     }
 
-    private static final double kMaxVisionJumpMeters = 1.0;
+    public void lockWheels() {
+        setControl(new SwerveRequest.SwerveDriveBrake());
+    }
+
+    public void aimToTarget() {
+        // 给未来命令预留的高层辅助接口，仍走现有底盘请求路径，不触碰模块底层逻辑。
+        double headingErrorDegrees = getHeadingError();
+        double omega = Math.toRadians(headingErrorDegrees) * Constants.AutoPositioning.TurningkP;
+        setControl(
+            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, omega))
+        );
+    }
+
     private static final double kMaxOmegaDegPerSec = 360.0;
 
     /**
-     * Returns a command that applies the specified control request to this swerve drivetrain.
+     * 返回一个持续下发指定 swerve 请求的命令。
      *
-     * @param request Function returning the request to apply
-     * @return Command to run
+     * @param request 生成待下发请求的函数。
+     * @return 可调度执行的 Command。
      */
     public Command applyRequest(Supplier<SwerveRequest> request) {
         return run(() -> this.setControl(request.get()));
@@ -184,6 +218,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
         addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
     }
+
     private void fuseLimelightmt2() {
         Pose2d current = getState().Pose;
 
@@ -202,32 +237,31 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
         LimelightHelpers.PoseEstimate mt2_final = null;
 
-        if(!shooterAvailable && !intakerAvailable) {
+        if (!shooterAvailable && !intakerAvailable) {
             positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
             positioningNetworkTable.getEntry("Usingll").setString("None");
-            //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
-            
+            // 未使用视觉时不发布距离差，避免仪表盘误读旧值。
             return;
-        }else if (shooterAvailable && !intakerAvailable) {
+        } else if (shooterAvailable && !intakerAvailable) {
             double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
             ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance);
             ll_shooter_NT.getEntry("X").setDouble(mt2_shooter.pose.getX());
             ll_shooter_NT.getEntry("Y").setDouble(mt2_shooter.pose.getY());
             positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
             positioningNetworkTable.getEntry("Usingll").setString("Shooter");
-            //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
+            // 单个 Limelight 可用时直接采用该测量。
             mt2_final = mt2_shooter;
-        }else if (!shooterAvailable && intakerAvailable) {
+        } else if (!shooterAvailable && intakerAvailable) {
             double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
             ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance);
             ll_intaker_NT.getEntry("X").setDouble(mt2_intaker.pose.getX());
             ll_intaker_NT.getEntry("Y").setDouble(mt2_intaker.pose.getY());
             positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
             positioningNetworkTable.getEntry("Usingll").setString("Intaker");
-            //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
+            // 单个 Limelight 可用时直接采用该测量。
             mt2_final = mt2_intaker;
-        }else {
-            // 两个都可用时的策略：这里简单选更近的，你也
+        } else {
+            // 两个 Limelight 都可用时，先选择与当前位姿更接近的测量。
             double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
             double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
             double difference = mt2_shooter.pose.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
@@ -258,11 +292,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         positioningNetworkTable.getEntry("PoseDistance").setDouble(distance);
         positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees()));
 
-        // 4) 设置测量噪声（示例：theta 基本不信任就给极大）
-        // 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调
+        // 视觉测量噪声：theta 基本不信任，只融合平移。
+        // 后续可以根据 mt2.avgTagDist / mt2.tagSpan 动态调整。
         var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
 
-        // 5) 融合：注意 timestampSeconds 的处理
+        // 融合视觉测量，注意 timestampSeconds 的时间基转换。
         addVisionMeasurement(mt2_final.pose, mt2_final.timestampSeconds, stdDevs);
         positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
     }
@@ -273,156 +307,28 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         }
         return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
     }
+
     private boolean ifLimelightPositioningAvailable(LimelightHelpers.PoseEstimate mt2, Pose2d currentPose) {
-        // 1) 没看到 tag：拒绝
+        // 没看到 tag 时拒绝该帧视觉定位。
         if (mt2 == null || mt2.tagCount == 0) {
             return false;
         }
-        // 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
-        double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
+
+        // 旋转太快时拒绝该帧，避免高速转动时的视觉跳变。
+        double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble();
         if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
-            //System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
             return false;
         }
 
-        // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
-        //Pose2d current = getState().Pose;
-        // if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
-        //     //System.out.println("NoUsingMetaTag2ForTooChangingPose");
-        //     return false;
-        // }
-
         return true;
     }
+
     private double clamp(double value, double min, double max) {
         return Math.max(min, Math.min(max, value));
     }
 
-//     public String driveToAprilTag() {
-//         System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
-//         System.out.println("TV = " + LimelightSupplier.isTargetVisible());
-
-//     if (!LimelightSupplier.isTargetVisible()) {
-//         isAlignedToAprilTag = false;
-//         //stop();
-//         return "No Target";
-//     }
-
-//     RawFiducial[] fiducials =
-//         LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-
-//     boolean foundDesiredTag = false;
-//     for (RawFiducial fiducial : fiducials) {
-//         if (fiducial.id == 16 || fiducial.id == 32) {
-//             foundDesiredTag = true;
-//             break;
-//         }
-//     }
-
-//     if (!foundDesiredTag) {
-//         isAlignedToAprilTag = false;
-//         stop();
-//         return "No Desired Tag";
-//     }
-
-//     // ===== 读取原始误差 =====
-//     double tx = LimelightSupplier.getTX();                 // deg
-//     double tz = -LimelightSupplier.getTargetTZ();          // m，方向按你当前经验保留负号
-//     double ry = LimelightSupplier.getTargetRotationY();    // deg
-
-//     // ===== 用原始误差判断是否已对齐 =====
-//     isAlignedToAprilTag =
-//         Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
-//         Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
-//         Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
-
-//     if (isAlignedToAprilTag) {
-//         stop();
-//         return "Aligned";
-//     }
-
-//     // ===== 计算输出 =====
-//     double vx = 0.0;
-//     double vy = 0.0;
-//     double omega = 0.0;
-
-//     // 1) 前后控制：根据 tz 前进/后退
-//     if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
-//         vx = tz * Constants.Limelight.AutoClimbKpTZ;
-
-//         // 最小输出，避免快到目标时不动
-//         if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
-//             vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
-//         }
-
-//         // 最大输出限幅
-//         vx = clamp(vx,
-//             -Constants.Limelight.AutoClimbMaxVX,
-//              Constants.Limelight.AutoClimbMaxVX);
-//     }
-
-//     // 2) 旋转控制：tx 为主，ry 为辅
-//     //    tx 负责“镜头中心对准 tag”
-//     //    ry 负责“机器人姿态微调”
-//     // double omegaFromTx = 0.0;
-//     // double omegaFromRy = 0.0;
-
-//     // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
-//     //     omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
-//     // }
-
-//     // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
-//     //     omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
-//     // }
-
-//     // omega = omegaFromTx + omegaFromRy;
-
-//     // // 大偏角时直接给较明显的转速，先把朝向拉回来
-//     // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
-//     //     omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
-//     // }
-
-//     // // 最小输出，避免转不动
-//     // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
-//     //     omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
-//     // }
-
-//     // // 最大输出限幅
-//     // omega = clamp(omega,
-//     //     -Constants.Limelight.AutoClimbMaxOmega,
-//     //      Constants.Limelight.AutoClimbMaxOmega);
-
-//     Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
-//     double climbCurrentRad = ClimbcurrentAngle.getRadians();
-//     double TargetAngleforClimbing = Math.toRadians(0);
-//     double Climberror = TargetAngleforClimbing - climbCurrentRad;
-//     if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
-//             omega = 0.0;
-//         } else {
-//             omega = Climberror * Constants.AutoPositioning.TurningkP;
-//         }
-
-
-//     // ===== 调试输出 =====
-//     driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
-//     driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
-//     driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
-//     driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
-//     driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
-//     driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
-
-//     // ===== 下发到底盘 =====
-//     driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
-
-//     return "Driving to AprilTag";
-// }
-
-//     public boolean isAlignedToAprilTag(){
-//         return isAlignedToAprilTag;
-//     }
     @Override
     public void periodic() {
-
         Pose2d pose = getState().Pose;
         m_field.setRobotPose(pose);
         posePub.set(pose);
@@ -431,20 +337,19 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
         driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
         driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
-        //positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
-        // positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
-        // positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
-        // positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
+
+        // 保留 Positioning 表中的基础状态输出。
         positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
-        // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
+
+        // 每帧向 Limelight 写入当前 yaw，供 MegaTag2 使用。
         LimelightHelpers.SetRobotOrientation(
             Constants.Limelight.LIMELIGHT_NAME_Shooter,
-            pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
+            pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
             0, 0, 0, 0, 0
         );
         LimelightHelpers.SetRobotOrientation(
             Constants.Limelight.LIMELIGHT_NAME_Intaker,
-            pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
+            pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
             0, 0, 0, 0, 0
         );
 
@@ -462,13 +367,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         }
     }
 
-        
     /**
-     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-     * while still accounting for measurement noise.
+     * 添加一帧视觉位姿测量，用于修正里程计估计。
      *
-     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param visionRobotPoseMeters 视觉测得的机器人位姿。
+     * @param timestampSeconds 视觉测量时间戳，单位秒。
      */
     @Override
     public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -476,17 +379,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
 
     /**
-     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-     * while still accounting for measurement noise.
-     * <p>
-     * Note that the vision measurement standard deviations passed into this method
-     * will continue to apply to future measurements until a subsequent call to
-     * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
+     * 添加一帧带标准差的视觉位姿测量。
      *
-     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-     * @param timestampSeconds The timestamp of the vision measurement in seconds.
-     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
-     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
+     * @param visionRobotPoseMeters 视觉测得的机器人位姿。
+     * @param timestampSeconds 视觉测量时间戳，单位秒。
+     * @param visionMeasurementStdDevs 视觉测量标准差，格式为 [x, y, theta]。
      */
     @Override
     public void addVisionMeasurement(
@@ -498,10 +395,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
 
     /**
-     * Return the pose at a given timestamp, if the buffer is not empty.
+     * 从位姿缓存中读取指定时间戳的机器人位姿。
      *
-     * @param timestampSeconds The timestamp of the pose in seconds.
-     * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+     * @param timestampSeconds 位姿时间戳，单位秒。
+     * @return 指定时间的位姿；缓存为空时返回 Optional.empty()。
      */
     @Override
     public Optional<Pose2d> samplePoseAt(double timestampSeconds) {

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

@@ -0,0 +1,60 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * 主 conveyor 的开环控制封装。
+ *
+ * 本赛季没有球检测传感器，因此这里仅报告电机状态；
+ * 所有动作顺序判断都放在 Superstructure。
+ */
+public class ConveyorSubsystem extends SubsystemBase {
+    public final TalonFX mainConveyor;
+
+    private final VoltageOut voltageRequest = new VoltageOut(0);
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
+
+    private double voltage = 0.0;
+
+    public ConveyorSubsystem() {
+        mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
+    }
+
+    public void feedToShooter() {
+        setVoltage(Constants.Conveyor.feedVoltage);
+    }
+
+    public void reverse() {
+        setVoltage(Constants.Conveyor.reverseVoltage);
+    }
+
+    public void hold() {
+        setVoltage(Constants.Conveyor.holdVoltage);
+    }
+
+    public void stop() {
+        setVoltage(0.0);
+    }
+
+    public void setPercent(double percent) {
+        setVoltage(percent * 12.0);
+    }
+
+    public void setVoltage(double volts) {
+        voltage = MathUtil.clamp(volts, -12.0, 12.0);
+        mainConveyor.setControl(voltageRequest.withOutput(voltage));
+    }
+
+    @Override
+    public void periodic() {
+        table.getEntry("voltage").setDouble(voltage);
+        table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
+    }
+}

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

@@ -0,0 +1,77 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * 进球滚轮的硬件封装。
+ *
+ * 何时 intake、outtake、hold 或 stop 由 Superstructure 决定；
+ * 本类只负责成对 TalonFX 输出和 follower 关系。
+ */
+public class IntakeRollerSubsystem extends SubsystemBase {
+    public final TalonFX leftIntakeRoller;
+    public final TalonFX rightIntakeRoller;
+
+    private final VoltageOut voltageRequest = new VoltageOut(0);
+    private final Follower rightFollower;
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller");
+
+    private double leftVoltage = 0.0;
+    private double rightVoltage = 0.0;
+
+    public IntakeRollerSubsystem() {
+        leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID);
+        rightIntakeRoller = new TalonFX(Constants.Intaker.rightIntakeRollerID);
+        rightFollower = new Follower(leftIntakeRoller.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        followLeft();
+    }
+
+    public void intake() {
+        setVoltage(Constants.Intaker.intakeRollerVoltage, -Constants.Intaker.intakeRollerVoltage);
+    }
+
+    public void outtake() {
+        setVoltage(Constants.Intaker.outtakeRollerVoltage, -Constants.Intaker.outtakeRollerVoltage);
+    }
+
+    public void hold() {
+        setVoltage(Constants.Intaker.holdRollerVoltage, -Constants.Intaker.holdRollerVoltage);
+    }
+
+    public void stop() {
+        setVoltage(0.0, 0.0);
+    }
+
+    public void setPercent(double left, double right) {
+        setVoltage(left * 12.0, right * 12.0);
+    }
+
+    public void setVoltage(double left, double right) {
+        // rightVoltage 仅用于 dashboard 显示；真实硬件通过反向 follower 跟随左侧电机。
+        leftVoltage = MathUtil.clamp(left, -12.0, 12.0);
+        rightVoltage = MathUtil.clamp(right, -12.0, 12.0);
+        leftIntakeRoller.setControl(voltageRequest.withOutput(leftVoltage));
+        followLeft();
+    }
+
+    private void followLeft() {
+        rightIntakeRoller.setControl(rightFollower);
+    }
+
+    @Override
+    public void periodic() {
+        table.getEntry("leftVoltage").setDouble(leftVoltage);
+        table.getEntry("rightVoltage").setDouble(rightVoltage);
+        table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble());
+        table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble());
+    }
+}

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

@@ -0,0 +1,145 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * 进球旋转臂的位置闭环和开环控制。
+ *
+ * 机构使用 TalonFX 集成编码器。归零前只允许 SAFE 或受限手动移动，
+ * 避免相对编码器未校准时发生大幅度意外旋转。
+ */
+public class IntakeRotaterSubsystem extends SubsystemBase {
+    public enum IntakePosition {
+        STOW(Constants.Intaker.intakeRotaterStowPosition),
+        GROUND_INTAKE(Constants.Intaker.intakeRotaterGroundPosition),
+        HANDOFF(Constants.Intaker.intakeRotaterHandoffPosition),
+        SAFE(Constants.Intaker.intakeRotaterSafePosition);
+
+        public final double rotations;
+
+        IntakePosition(double rotations) {
+            this.rotations = rotations;
+        }
+    }
+
+    public final TalonFX leftIntakeRotater;
+    public final TalonFX rightIntakeRotater;
+
+    private final PositionVoltage positionRequest = new PositionVoltage(0);
+    private final VoltageOut voltageRequest = new VoltageOut(0);
+    private final Follower rightFollower;
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater");
+
+    private IntakePosition goal = IntakePosition.SAFE;
+    private boolean zeroed = false;
+    private double manualVoltage = 0.0;
+
+    public IntakeRotaterSubsystem() {
+        leftIntakeRotater = new TalonFX(Constants.Intaker.leftIntakeRotaterID);
+        rightIntakeRotater = new TalonFX(Constants.Intaker.rightIntakeRotaterID);
+        leftIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
+        rightIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
+        rightFollower = new Follower(leftIntakeRotater.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        followLeft();
+    }
+
+    public void setGoal(IntakePosition position) {
+        // 相对编码器没有对准机械零点前，禁止执行大幅度位置闭环移动。
+        goal = position;
+        if (!zeroed && position != IntakePosition.SAFE) {
+            stop();
+            return;
+        }
+
+        double target = MathUtil.clamp(
+            position.rotations,
+            Constants.Intaker.intakeRotaterDownLimit,
+            Constants.Intaker.intakeRotaterUpLimit
+        );
+        leftIntakeRotater.setControl(positionRequest.withPosition(target));
+        manualVoltage = 0.0;
+        followLeft();
+    }
+
+    public void stow() {
+        setGoal(IntakePosition.STOW);
+    }
+
+    public void deployToGround() {
+        setGoal(IntakePosition.GROUND_INTAKE);
+    }
+
+    public void moveToHandoff() {
+        setGoal(IntakePosition.HANDOFF);
+    }
+
+    public boolean atGoal() {
+        return Math.abs(getPosition() - goal.rotations) <= Constants.Intaker.positionToleranceRotations;
+    }
+
+    public void zeroPosition() {
+        // 只有 intake 物理上处于已知零点姿态时才调用此方法。
+        leftIntakeRotater.setPosition(0.0);
+        rightIntakeRotater.setPosition(0.0);
+        zeroed = true;
+        goal = IntakePosition.STOW;
+        stow();
+    }
+
+    public boolean isZeroed() {
+        return zeroed;
+    }
+
+    public void clearZeroed() {
+        zeroed = false;
+        goal = IntakePosition.SAFE;
+        stop();
+    }
+
+    public double getPosition() {
+        return leftIntakeRotater.getPosition().getValueAsDouble();
+    }
+
+    public IntakePosition getGoal() {
+        return goal;
+    }
+
+    public void setOpenLoopVoltage(double voltage) {
+        // 归零前允许手动小幅移动，但输出会被限制到较温和的范围。
+        if (!zeroed && Math.abs(voltage) > Constants.Intaker.intakeRotaterManualVoltage) {
+            voltage = Math.copySign(Constants.Intaker.intakeRotaterManualVoltage, voltage);
+        }
+        manualVoltage = MathUtil.clamp(voltage, -12.0, 12.0);
+        leftIntakeRotater.setControl(voltageRequest.withOutput(manualVoltage));
+        followLeft();
+    }
+
+    public void stop() {
+        manualVoltage = 0.0;
+        leftIntakeRotater.setControl(voltageRequest.withOutput(0.0));
+        followLeft();
+    }
+
+    private void followLeft() {
+        rightIntakeRotater.setControl(rightFollower);
+    }
+
+    @Override
+    public void periodic() {
+        table.getEntry("goal").setString(goal.name());
+        table.getEntry("zeroed").setBoolean(zeroed);
+        table.getEntry("position").setDouble(getPosition());
+        table.getEntry("atGoal").setBoolean(atGoal());
+        table.getEntry("manualVoltage").setDouble(manualVoltage);
+    }
+}

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

@@ -1,100 +0,0 @@
-package frc.robot.subsystems;
-
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
-
-public class IntakerSubsystem extends SubsystemBase{
-    //定义intakerotater&intakegetter
-    private TalonFX intakerotater;
-    private TalonFX intakegetter;
-
-    //PID 
-    private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
-    private final PositionVoltage positionRequest = new PositionVoltage(0);
- 
-    private double intakeRotaterTargetPosition = 0.0;
-
-    public double getGravityFFVolts(){
-        double intakerotaterPosition = getIntakerotaterPosition();
-        double gravityFFVolts = Constants.Intaker.IntakeRotaterGravityFF * intakerotaterPosition;
-        return gravityFFVolts;
-    }
-
-    public void setIntakerRotaterVelocityRps(double IntakeRotaterTargetVelocity) {
-        double gravityFeedforward = getGravityFFVolts();
-        intakerotater.setControl(velocityRequest.withVelocity(IntakeRotaterTargetVelocity).withFeedForward(gravityFeedforward));// 重力前馈);
-    } 
-
-    public IntakerSubsystem() {
-        //初始化rotater&getter
-        intakegetter = new TalonFX(47);
-        intakerotater = new TalonFX(48);
-        intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
-        intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);      
-    }
-
-    public void setIntakerRotaterPosition(double targetPosition) {
-        //目标position
-        if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) {
-            targetPosition = Constants.Intaker.intakeRotaterUpLimit;  // 限制最大上限
-        } else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) {
-            targetPosition = Constants.Intaker.intakeRotaterDownLimit;  // 限制最小下限
-        }
-
-        intakeRotaterTargetPosition = targetPosition;
-        intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
-    }
-
-    // 获取intakerotater当前角度
-    public double getIntakerotaterPosition() {
-        return intakerotater.getPosition().getValueAsDouble();
-    }
-
-    public boolean isIntakerotaterAtTarget() {
-        // 到位判断容差
-        double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition());
-        return error <= Constants.Intaker.positionToleranceRotations;
-    }
-
-     public void stopIntakerotater() {
-        intakerotater.set(0);
-        // 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
-    }
-
-    //intakegetter的on/off
-    // public void setIntakeGetterOn(boolean on) {
-    //     if (on) {
-    //         intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
-    //     }
-    //     else {
-    //         intakegetter.setControl(velocityRequest.withVelocity(0));
-    //     }
-    // }
-    public void setIntakerGetterSpeed(double speed) {
-        intakegetter.set(speed);
-    }
-    public void zeroEncoder(){
-        intakerotater.setPosition(0);
-        stopIntakerotater();
-    }
-    public void IntakerDownNonStop(){
-            intakerotater.set(-0.1);
-        }
-
-    public void IntakerUpNonStop(){
-            intakerotater.set(0.2);
-        }
-
-    public void setIntakerRotater(double rotaterspeed){
-        intakerotater.set(rotaterspeed);
-    }
-    public void stopAll() {
-        stopIntakerotater();
-        setIntakerGetterSpeed(0);
-    }
-}

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

@@ -0,0 +1,55 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * 对现有 LimelightHelpers 的薄只读封装。
+ *
+ * 本类不做视觉融合，也不控制任何机构；只给 Superstructure 提供稳定的
+ * 目标和距离读取接口。
+ */
+public class LimelightSubsystem extends SubsystemBase {
+    private final String limelightName;
+
+    public LimelightSubsystem() {
+        this(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+    }
+
+    public LimelightSubsystem(String limelightName) {
+        this.limelightName = limelightName;
+    }
+
+    public boolean hasTarget() {
+        return LimelightHelpers.getTV(limelightName);
+    }
+
+    public double getTargetYaw() {
+        return LimelightHelpers.getTX(limelightName);
+    }
+
+    public double getTargetPitch() {
+        return LimelightHelpers.getTY(limelightName);
+    }
+
+    public double getDistanceToTarget() {
+        // DriveControls 已经把基于场地位姿计算的距离发布到 AutoControl；
+        // 这里复用该值，保留旧定位和目标计算逻辑。
+        return NetworkTableInstance.getDefault()
+            .getTable("AutoControl")
+            .getEntry("distanceToHub")
+            .getDouble(0.0);
+    }
+
+    public Pose2d getEstimatedPose() {
+        return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
+    }
+
+    public double getShooterSetpointByDistance() {
+        // 初始测试用临时映射；飞轮和背板调好后应替换为实测射击表。
+        double distance = getDistanceToTarget();
+        return Constants.Shooter.defaultFlywheelTargetRps + (distance - 2.5) * 4.0;
+    }
+}

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

@@ -3,15 +3,15 @@ package frc.robot.subsystems;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.hardware.TalonFXS;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -19,89 +19,188 @@ import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
+/**
+ * Shooter 总成的硬件封装。
+ *
+ * 飞轮和 shooter 内部 conveyor 都使用电压开环输出。
+ * 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。
+ */
 public class ShooterSubsystem extends SubsystemBase {
-    // 定义飞轮、传动和背板电机
-    public TalonFX flywheelMotorLeft;
-    private TalonFX flywheelMotorRight;
-    public TalonFX conveyorMotor;
-    public TalonFXS backboardMotor;
-    public Encoder backboardEncoder;
+    public final TalonFX leftFlywheel;
+    public final TalonFX rightFlywheel;
+    public final TalonFX leftConveyor;
+    public final TalonFX rightConveyor;
 
-    // PID 控制器，控制背板角度
-    public ProfiledPIDController backboardPID;
-    private Follower flyWheelFollower;
+    // 保留旧字段别名，让原有 dashboard tuner 在重构后仍能指向左飞轮和左 conveyor。
+    public final TalonFX flywheelMotorLeft;
+    public final TalonFX conveyorMotor;
 
-    private NetworkTable shooterNetworkTable;
+    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 Follower flywheelFollower;
+    private final Follower conveyorFollower;
+
+    private final NetworkTable shooterNetworkTable;
+
+    private double targetFlywheelRps = 0.0;
+    private double targetLeftFlywheelRps = 0.0;
+    private double targetRightFlywheelRps = 0.0;
+    private double shooterConveyorPercent = 0.0;
 
-    // 初始化各个部件
     public ShooterSubsystem() {
-        // 初始化飞轮电机
-        flywheelMotorLeft = new TalonFX(41);  // 41是飞轮left的CAN ID
-        flywheelMotorRight = new TalonFX(42);  // 42是飞轮right的CAN ID
-        flywheelMotorLeft.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
-        flywheelMotorRight.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
-        // 初始化传动电机
-        conveyorMotor = new TalonFX(43);
-        conveyorMotor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
+        leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID);
+        rightFlywheel = new TalonFX(Constants.Shooter.rightFlywheelID);
+        leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID);
+        rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID);
 
-        var configs1 = new MotorOutputConfigs();
-        configs1.Inverted = InvertedValue.Clockwise_Positive;
-        flywheelMotorLeft.getConfigurator().apply(configs1);
-        flyWheelFollower = new Follower(flywheelMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        flywheelMotorLeft = leftFlywheel;
+        conveyorMotor = leftConveyor;
 
-        var configs2 = new MotorOutputConfigs();
-        configs2.Inverted = InvertedValue.Clockwise_Positive;
-        conveyorMotor.getConfigurator().apply(configs2);
+        leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
+        rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
+        leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
+        rightConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
 
-        // 初始化背板电机和编码器
-        backboardMotor = new TalonFXS(44);  
+        var motorOutputConfigs = new MotorOutputConfigs();
+        motorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
+        leftFlywheel.getConfigurator().apply(motorOutputConfigs);
+        leftConveyor.getConfigurator().apply(motorOutputConfigs);
+
+        flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        setRightFollowLeft();
+
+        backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
         backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
+
         backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
         backboardEncoder.setSamplesToAverage(5);
         backboardEncoder.setMinRate(1.0);
         backboardEncoder.setDistancePerPulse(1);
 
-        // 初始化背板的PID控制器
-        backboardPID = new ProfiledPIDController(Constants.Shooter.backboardPositionPID.kP, Constants.Shooter.backboardPositionPID.kI, Constants.Shooter.backboardPositionPID.kD,
-                                                    new Constraints(10000,100000));
-        
+        backboardPID = new ProfiledPIDController(
+            Constants.Shooter.backboardPositionPID.kP,
+            Constants.Shooter.backboardPositionPID.kI,
+            Constants.Shooter.backboardPositionPID.kD,
+            new Constraints(10000, 100000)
+        );
         backboardPID.setTolerance(10.0);
 
         shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
-
     }
 
-    public void setRightFollowLeft(){
-        flywheelMotorRight.setControl(flyWheelFollower);
+    public void setRightFollowLeft() {
+        // 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
+        rightFlywheel.setControl(flywheelFollower);
+        rightConveyor.setControl(conveyorFollower);
     }
 
-    public void applyLeftConfigurationToRight(){
+    public void applyLeftConfigurationToRight() {
         Slot0Configs configs = new Slot0Configs();
-        flywheelMotorLeft.getConfigurator().refresh(configs);
-        flywheelMotorRight.getConfigurator().apply(configs);
+        leftFlywheel.getConfigurator().refresh(configs);
+        rightFlywheel.getConfigurator().apply(configs);
     }
 
+    public void setFlywheelVelocity(double leftRps, double rightRps) {
+        // 对外接口使用 RPS，便于 Superstructure 阅读；实际下发仍是电压开环。
+        targetLeftFlywheelRps = leftRps;
+        targetRightFlywheelRps = rightRps;
+        targetFlywheelRps = (Math.abs(leftRps) + Math.abs(rightRps)) / 2.0;
 
-    // 设置飞轮的速度
-    public void setFlywheelSpeedByRPS(double targetSpeed) {
-        flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒
+        double requestedVoltage = targetFlywheelRps * Constants.Shooter.flywheelVoltsPerRps;
+        requestedVoltage = MathUtil.clamp(requestedVoltage, -12.0, 12.0);
+        leftFlywheel.setControl(flywheelVoltageRequest.withOutput(requestedVoltage));
         setRightFollowLeft();
-        // System.out.println("set flywheel "+targetSpeed);
     }
 
-    // 设置传动的速度
+    public void setFlywheelVelocityByDistance(double distance) {
+        // 临时距离映射。拿到真实射击数据后，应替换为实测表或重新调常量。
+        double clampedDistance = MathUtil.clamp(distance, 1.2, 5.0);
+        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;
+        targetFlywheelRps = 0.0;
+        leftFlywheel.setControl(flywheelVoltageRequest.withOutput(MathUtil.clamp(volts, -12.0, 12.0)));
+        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) {
-        conveyorMotor.setControl(velocityRequest.withVelocity(speed));  // 设置传动电机速度
-    }
-    public void setConveyorSpeedOpen(double speed){
-        conveyorMotor.set(speed);
+        leftConveyor.setControl(velocityRequest.withVelocity(speed));
+        setRightFollowLeft();
     }
 
-    //调试用
-    public void setBackboardSpeedByRPS(double speed){
+    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) {
+            return false;
+        }
+        return Math.abs(getLeftFlywheelVelocity() - targetLeftFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps
+            && Math.abs(Math.abs(getRightFlywheelVelocity()) - Math.abs(targetRightFlywheelRps)) <= Constants.Shooter.flywheelSpeedToleranceRps;
+    }
+
+    public double getLeftFlywheelVelocity() {
+        return leftFlywheel.getVelocity().getValueAsDouble();
+    }
+
+    public double getRightFlywheelVelocity() {
+        return rightFlywheel.getVelocity().getValueAsDouble();
+    }
+
+    public double getAverageFlywheelVelocity() {
+        return (Math.abs(getLeftFlywheelVelocity()) + Math.abs(getRightFlywheelVelocity())) / 2.0;
+    }
+
+    public double getFlywheelVelocityDifference() {
+        return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
+    }
+
+    public double getTargetFlywheelRps() {
+        return targetFlywheelRps;
+    }
+
+    public void setBackboardSpeedByRPS(double speed) {
         if (speed > 0 && speed < 10) {
             speed = 10;
         } else if (speed < 0 && speed > -10) {
@@ -110,67 +209,71 @@ public class ShooterSubsystem extends SubsystemBase {
         backboardMotor.setControl(velocityRequest.withVelocity(speed));
     }
 
-    // 设置背板的目标位置，通过PID控制器来调整  范围0-3000
     public void setBackboardPosition(double targetPosition) {
-        // 限制目标位置在上下限之间
+        // 保留旧背板机构的软件限位。
         if (targetPosition > Constants.Shooter.backboardUpLimit) {
-            targetPosition = Constants.Shooter.backboardUpLimit;  // 限制最大上限
+            targetPosition = Constants.Shooter.backboardUpLimit;
         } else if (targetPosition < Constants.Shooter.backboardDownLimit) {
-            targetPosition = Constants.Shooter.backboardDownLimit;  // 限制最小下限
+            targetPosition = Constants.Shooter.backboardDownLimit;
         }
         backboardPID.setGoal(targetPosition);
     }
-    public void outputBackboard(){
 
-        // 使用PID控制器来计算背板的运动速度
+    public void outputBackboard() {
+        // 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
         double pidOutput = backboardPID.calculate(getBackboardPosition());
-        pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围
+        pidOutput = MathUtil.clamp(pidOutput, -Constants.Shooter.backboardSpeedMax, Constants.Shooter.backboardSpeedMax);
 
         if (Math.abs(pidOutput) < 0.8) {
             pidOutput = 0.0;
-        }//防止抖动
-        // // 设置背板电机的速度，根据PID输出进行调整
+        }
+
         shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
         setBackboardSpeedByRPS(pidOutput);
     }
-    // 停止所有电机
-    public void stopMotors() {
-        flywheelMotorLeft.set(0);
-        flywheelMotorRight.set(0);
-        conveyorMotor.set(0);
-        backboardMotor.set(0);
-    }
 
-    // 获取背板当前角度
     public double getBackboardPosition() {
-        return backboardEncoder.getDistance();  // 返回背板电机编码器的当前角度
+        return backboardEncoder.getDistance();
     }
 
-    // 检查背板是否到达目标角度
     public boolean isBackboardAtTarget() {
-        return backboardPID.atGoal();  // 判断背板是否到达目标角度
+        return backboardPID.atGoal();
     }
-    public void conveyorWaitForAcceleration(){
-        conveyorMotor.set(0);
+
+    public void conveyorWaitForAcceleration() {
+        stopShooterConveyor();
     }
-    public void conveyorRun(){
-        setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
+
+    public void conveyorRun() {
+        runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
     }
-    public void resetbackboardencoder(){
+
+    public void resetbackboardencoder() {
         backboardEncoder.reset();
     }
-    public double getConveyorStatorCurrent(){
-        double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble();
-        return statorCurrent;
+
+    public double getConveyorStatorCurrent() {
+        return leftConveyor.getStatorCurrent().getValueAsDouble();
+    }
+
+    public double getDistanceToHub() {
+        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
+    }
+
+    public double getDistanceToPassball() {
+        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
     }
 
     @Override
     public void periodic() {
         backboardPID.calculate(getBackboardPosition());
-        shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble());
-        shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble());
-        shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble());
-        shooterNetworkTable.getEntry("conveyorPIDReference").setDouble(conveyorMotor.getClosedLoopReference().getValueAsDouble());
+        shooterNetworkTable.getEntry("flywheelSpeed").setDouble(getAverageFlywheelVelocity());
+        shooterNetworkTable.getEntry("leftFlywheelSpeed").setDouble(getLeftFlywheelVelocity());
+        shooterNetworkTable.getEntry("rightFlywheelSpeed").setDouble(getRightFlywheelVelocity());
+        shooterNetworkTable.getEntry("flywheelTargetSpeed").setDouble(targetFlywheelRps);
+        shooterNetworkTable.getEntry("flywheelAtSpeed").setBoolean(atSpeed());
+        shooterNetworkTable.getEntry("shooterConveyorPercent").setDouble(shooterConveyorPercent);
+        shooterNetworkTable.getEntry("conveyorSpeed").setDouble(leftConveyor.getVelocity().getValueAsDouble());
         shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
         shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
         shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
@@ -178,17 +281,4 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
         shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
     }
-
-    public double getDistanceToHub(){
-        //获得车子距离Hub的距离
-        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
-    }
-    public double getDistanceToPassball(){
-        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
-    }
-    public void stopAll() {
-        setFlywheelSpeedByRPS(0);
-        setConveyorSpeedByRPS(0);
-        backboardMotor.set(0);
-    }
 }

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

@@ -0,0 +1,342 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+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 距离如何配合，
+ * 统一由这里决定。
+ */
+public class Superstructure extends SubsystemBase {
+    // 操作员或自动程序请求的目标状态。
+    public enum WantedState {
+        IDLE,
+        INTAKE,
+        PREP_SHOOT,
+        SHOOT,
+        EJECT,
+        MANUAL
+    }
+
+    // 机器人内部实际执行状态，用于动作排序和仪表盘诊断。
+    public enum SystemState {
+        DISABLED,
+        STOWED,
+        DEPLOYING_INTAKE,
+        INTAKING,
+        SPINNING_UP,
+        AIMING,
+        READY_TO_SHOOT,
+        SHOOTING,
+        CLEANUP,
+        EJECTING,
+        MANUAL_OVERRIDE
+    }
+
+    private final ShooterSubsystem shooter;
+    private final IntakeRollerSubsystem intakeRoller;
+    private final IntakeRotaterSubsystem intakeRotater;
+    private final ConveyorSubsystem conveyor;
+    private final CommandSwerveDrivetrain drive;
+    private final LimelightSubsystem limelight;
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Superstructure");
+
+    private WantedState wantedState = WantedState.IDLE;
+    private SystemState systemState = SystemState.STOWED;
+    private double stateStartTimestamp = Timer.getFPGATimestamp();
+    private double flywheelSpeedOffsetRps = 0.0;
+    private double backboardPositionOffset = 0.0;
+
+    public Superstructure(
+        ShooterSubsystem shooter,
+        IntakeRollerSubsystem intakeRoller,
+        IntakeRotaterSubsystem intakeRotater,
+        ConveyorSubsystem conveyor,
+        CommandSwerveDrivetrain drive,
+        LimelightSubsystem limelight
+    ) {
+        this.shooter = shooter;
+        this.intakeRoller = intakeRoller;
+        this.intakeRotater = intakeRotater;
+        this.conveyor = conveyor;
+        this.drive = drive;
+        this.limelight = limelight;
+    }
+
+    public void requestIdle() {
+        wantedState = WantedState.IDLE;
+    }
+
+    public void requestIntake() {
+        wantedState = WantedState.INTAKE;
+    }
+
+    public void requestPrepShoot() {
+        wantedState = WantedState.PREP_SHOOT;
+    }
+
+    public void requestShoot() {
+        wantedState = WantedState.SHOOT;
+    }
+
+    public void requestEject() {
+        wantedState = WantedState.EJECT;
+    }
+
+    public void requestManual() {
+        wantedState = WantedState.MANUAL;
+    }
+
+    public Command requestIdleCommand() {
+        return Commands.runOnce(this::requestIdle, this);
+    }
+
+    public Command requestIntakeCommand() {
+        return Commands.runOnce(this::requestIntake, this);
+    }
+
+    public Command requestPrepShootCommand() {
+        return Commands.runOnce(this::requestPrepShoot, this);
+    }
+
+    public Command requestShootCommand() {
+        return Commands.runOnce(this::requestShoot, this);
+    }
+
+    public Command requestEjectCommand() {
+        return Commands.runOnce(this::requestEject, this);
+    }
+
+    public Command requestManualCommand() {
+        return Commands.runOnce(this::requestManual, this);
+    }
+
+    public WantedState getWantedState() {
+        return wantedState;
+    }
+
+    public SystemState getSystemState() {
+        return systemState;
+    }
+
+    public boolean canShoot() {
+        // 本赛季不使用球检测传感器，因此射击门控只检查机器人能实际测量的条件。
+        return shooter.atSpeed()
+            && drive.isAimed();
+    }
+
+    public void adjustFlywheelSpeedOffset(double offsetRps) {
+        flywheelSpeedOffsetRps += offsetRps;
+    }
+
+    public void adjustBackboardPositionOffset(double offset) {
+        backboardPositionOffset += offset;
+    }
+
+    public void resetOffsets() {
+        flywheelSpeedOffsetRps = 0.0;
+        backboardPositionOffset = 0.0;
+    }
+
+    public void resetBackboardEncoder() {
+        shooter.resetbackboardencoder();
+    }
+
+    @Override
+    public void periodic() {
+        // 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。
+        if (DriverStation.isDisabled()) {
+            setSystemState(SystemState.DISABLED);
+            stopAllMechanisms();
+            publishTelemetry();
+            return;
+        }
+
+        updateAimingSetpoint();
+
+        switch (wantedState) {
+            case IDLE -> handleIdle();
+            case INTAKE -> handleIntake();
+            case PREP_SHOOT -> handlePrepShoot(false);
+            case SHOOT -> handlePrepShoot(true);
+            case EJECT -> handleEject();
+            case MANUAL -> handleManual();
+        }
+
+        publishTelemetry();
+    }
+
+    private void handleIdle() {
+        // 安全默认状态：传球不动，intake 收回，背板保持受控。
+        conveyor.hold();
+        intakeRoller.stop();
+        intakeRotater.stop();
+        shooter.stopFlywheel();
+        shooter.stopShooterConveyor();
+        holdBackboardAtZero();
+
+        setSystemState(SystemState.STOWED);
+    }
+
+    private void handleIntake() {
+        if (systemState != SystemState.DEPLOYING_INTAKE
+            && systemState != SystemState.INTAKING) {
+            setSystemState(SystemState.DEPLOYING_INTAKE);
+        }
+
+        shooter.stopFlywheel();
+        shooter.stopShooterConveyor();
+        intakeRotater.deployToGround();
+        intakeRoller.intake();
+        conveyor.feedToShooter();
+        holdBackboardAtZero();
+
+        if (intakeRotater.atGoal()) {
+            setSystemState(SystemState.INTAKING);
+        }
+
+        // 本赛季没有球检测，因此 intake 通过时间退出。
+        // 真车确认 roller/conveyor 速度后，需要现场调整这个时间。
+        if (systemState == SystemState.INTAKING && timeInState() > 1.0) {
+            wantedState = WantedState.IDLE;
+            setSystemState(SystemState.STOWED);
+        }
+    }
+
+    private void handlePrepShoot(boolean feedWhenReady) {
+        if (systemState != SystemState.SPINNING_UP
+            && systemState != SystemState.AIMING
+            && systemState != SystemState.READY_TO_SHOOT
+            && systemState != SystemState.SHOOTING
+            && systemState != SystemState.CLEANUP) {
+            setSystemState(SystemState.SPINNING_UP);
+        }
+
+        intakeRotater.moveToHandoff();
+        intakeRoller.hold();
+        conveyor.hold();
+        shooter.stopShooterConveyor();
+        // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
+        // 但不会绕过 Superstructure 状态机。
+        double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
+        shooter.setFlywheelVelocity(targetRps, targetRps);
+        holdBackboardForShot();
+
+        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() 通过后才允许喂球。
+            conveyor.feedToShooter();
+            shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
+
+            if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
+                setSystemState(SystemState.CLEANUP);
+            }
+        }
+
+        if (systemState == SystemState.CLEANUP) {
+            conveyor.stop();
+            shooter.stopShooterConveyor();
+            if (timeInState() > Constants.Superstructure.cleanupSeconds) {
+                wantedState = WantedState.IDLE;
+                setSystemState(SystemState.STOWED);
+            }
+        }
+    }
+
+    private void handleEject() {
+        // Eject 会反转整条球路，但飞轮保持停止。
+        setSystemState(SystemState.EJECTING);
+        intakeRotater.deployToGround();
+        intakeRoller.outtake();
+        conveyor.reverse();
+        shooter.runShooterConveyor(Constants.Superstructure.shooterReversePercent);
+        shooter.stopFlywheel();
+        holdBackboardAtZero();
+    }
+
+    private void handleManual() {
+        setSystemState(SystemState.MANUAL_OVERRIDE);
+        shooter.stopFlywheel();
+        shooter.stopShooterConveyor();
+        conveyor.stop();
+        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();
+        intakeRotater.stop();
+        conveyor.stop();
+    }
+
+    private void setSystemState(SystemState nextState) {
+        if (systemState != nextState) {
+            systemState = nextState;
+            stateStartTimestamp = Timer.getFPGATimestamp();
+        }
+    }
+
+    private double timeInState() {
+        return Timer.getFPGATimestamp() - stateStartTimestamp;
+    }
+
+    private void publishTelemetry() {
+        // 发布状态和门控信号，方便判断机器人为什么还没进入射击。
+        table.getEntry("wantedState").setString(wantedState.name());
+        table.getEntry("systemState").setString(systemState.name());
+        table.getEntry("canShoot").setBoolean(canShoot());
+        table.getEntry("shooterAtSpeed").setBoolean(shooter.atSpeed());
+        table.getEntry("driveAimed").setBoolean(drive.isAimed());
+        table.getEntry("intakeAtGoal").setBoolean(intakeRotater.atGoal());
+        table.getEntry("flywheelSpeedOffsetRps").setDouble(flywheelSpeedOffsetRps);
+        table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
+    }
+}
+