shoot逻辑修复 新老状态机合并

src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto

@@ -13,13 +13,13 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         }
       ]

src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto

@@ -13,7 +13,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Copy of Shoot and Depot Blue.auto

@@ -30,7 +30,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "shoot"
+                  "name": "shoot_hub"
                 }
               },
               {
@@ -100,7 +100,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {
@@ -125,7 +125,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {

src/main/deploy/pathplanner/autos/Depot Auto Blue.auto

@@ -76,7 +76,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Depot Auto Red.auto

@@ -76,7 +76,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         }
       ]

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -19,7 +19,7 @@
         {
           "type": "named",
           "data": {
-            "name": "AutoShoot"
+            "name": "shoot_hub"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto

@@ -13,7 +13,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         },
         {

src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto

@@ -26,7 +26,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         },
         {

src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto

@@ -30,7 +30,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "shoot"
+                  "name": "shoot_hub"
                 }
               },
               {
@@ -100,7 +100,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {
@@ -125,7 +125,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {

src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto

@@ -26,7 +26,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         },
         {
@@ -99,7 +99,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {
@@ -124,7 +124,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {

src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto

@@ -32,7 +32,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         },
         {
@@ -93,7 +93,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {

src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto

@@ -32,7 +32,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         },
         {
@@ -93,7 +93,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {

src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto

@@ -32,7 +32,7 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "shoot_hub"
           }
         },
         {
@@ -93,7 +93,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoShoot"
+                  "name": "shoot_hub"
                 }
               },
               {

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

@@ -10,15 +10,6 @@ public final class Constants {
 
     public static final boolean LEDUsing = false;
 
-    public enum RobotStatus {
-        Stopped,
-        AllTelop,
-        PassingBall,
-        AutoAimming,
-        CrossingBump,
-        CrossingTrench
-    }
-
     public static class Shooter {
         // 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。
         public static final int leftFlywheelID = 41;
@@ -166,10 +157,11 @@ public final class Constants {
         // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
         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;
+        public static final double passBallFlywheelRps = Shooter.defaultFlywheelTargetRps;
+        public static final double passBallBackboardPosition = 175.0;
 
         // 射击时背板的目标位置（外接编码器计数）。
         public static final double backboardShootPosition = 175.0;

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

@@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Commands;
 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.OperatorControls;
@@ -67,13 +66,11 @@ public class RobotContainer {
   // 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);
+  public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, superstructure);
 
   // 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
   private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
@@ -151,26 +148,19 @@ public class RobotContainer {
     }));
 
     // 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
-    // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
+    // controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand());
-    // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    // controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand());
 
     // 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。
     Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。
-    autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
+    autoAim.onTrue(operatorControls.requestAimHubCommand());
-    autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    autoAim.onFalse(operatorControls.requestIdleCommand());
 
     // 上操作员左保险：按住进入传球模式，松开回到全手动模式。
     Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。
-    autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
+    autoAim2.onTrue(operatorControls.requestPassBallCommand());
-    autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    autoAim2.onFalse(operatorControls.requestIdleCommand());
 
-    controllerlower.leftTrigger()
-        .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
-        .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
-
-    controllerlower.rightTrigger()
-        .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
-        .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
   }
 
   private void configueShooter() {
@@ -180,10 +170,7 @@ public class RobotContainer {
     // 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
     Trigger teleopOnly = RobotModeTriggers.teleop();
     controllerupper.rightTrigger().and(teleopOnly)
-        .onTrue(operatorControls.requestShootCommand())
+        .onTrue(operatorControls.requestShootHubCommand())
-        .onFalse(operatorControls.requestIdleCommand());
-    controllerupper.y().and(teleopOnly)
-        .onTrue(operatorControls.requestPrepShootCommand())
         .onFalse(operatorControls.requestIdleCommand());
     controllerupper.b().and(teleopOnly)
         .onTrue(operatorControls.requestEjectCommand())

src/main/java/frc/robot/RobotStatusManager.java

@@ -1,25 +0,0 @@
-package frc.robot;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.Constants.RobotStatus;
-
-public class RobotStatusManager {
-    private RobotStatus robotStatus;
-    
-    public RobotStatusManager(){
-        this.robotStatus = RobotStatus.AllTelop;
-    }
-
-    public RobotStatus getStatus(){
-        return this.robotStatus;
-    }
-
-    public void setStatus(RobotStatus rs){
-        this.robotStatus = rs;
-    }
-
-    public Command setStatusCommand(RobotStatus rs){
-        return Commands.runOnce(()->setStatus(rs));
-    }
-}

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

@@ -5,6 +5,7 @@ import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
 import frc.robot.subsystems.IntakeRotaterSubsystem;
 import frc.robot.subsystems.Superstructure;
 
@@ -28,10 +29,9 @@ public class Autocommand extends SequentialCommandGroup {
         NamedCommands.registerCommand("stop_intake", idle(superstructure));
         NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater));
         NamedCommands.registerCommand("intaker_up", idle(superstructure));
-        NamedCommands.registerCommand("shoot", shoot(superstructure));
+        NamedCommands.registerCommand("shoot_hub", shootHub(superstructure));
         NamedCommands.registerCommand("Shake", shake(driveControls));
         NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
-        NamedCommands.registerCommand("AutoShoot", operatorControls.autoShootToHubCommand());
         NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command());
     }
 
@@ -56,12 +56,10 @@ public class Autocommand extends SequentialCommandGroup {
     }
 
     // 自动射击和手动射击使用同一套 gated shooting 流程。
-    public static Command shoot(Superstructure superstructure) {
+    public static Command shootHub(Superstructure superstructure) {
         return Commands.sequence(
-            superstructure.requestPrepShootCommand(),
+            superstructure.requestShootHubCommand(),
-            Commands.waitUntil(superstructure::canShoot).withTimeout(1.5),
+            Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
-            superstructure.requestShootCommand(),
-            Commands.waitSeconds(1.2),
             superstructure.requestIdleCommand()
         );
     }

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

@@ -15,11 +15,11 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
-import frc.robot.RobotStatusManager;
 import frc.robot.Constants.DriveMode;
-import frc.robot.Constants.RobotStatus;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Superstructure;
+import frc.robot.subsystems.Superstructure.WantedState;
 
 import java.util.Optional;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -36,35 +36,35 @@ public class DriveControls {
     private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
     private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
 
-    // 默认速度比例（平时开车）
+    // 榛樿閫熷害姣斾緥锛堝钩鏃跺紑杞︼級
     private double normalDriveScale = 0.3;
     private double normalTurnScale  = 0.7;
 
-    // 加速模式比例（按住 RB）
+    // 鍔犻€熸ā寮忔瘮渚嬶紙鎸変綇 RB锛?
     private double boostDriveScale = 1.0;
     private double boostTurnScale  = 1.0;
-    //autoaim模式
+    //autoaim妯″紡
     private double slowDriveScale =0.1;
 
     private boolean boostEnabled = false;
 
-    private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0);   // m/s^2 等效（调）
+    private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0);   // m/s^2 绛夋晥锛堣皟锛?
     private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
-    private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调）
+    private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 绛夋晥锛堣皟锛?
 
-    // ====== 类成员变量里新增：记录上一次输出 ======
+    // ====== 绫绘垚鍛樺彉閲忛噷鏂板锛氳褰曚笂涓€娆¤緭鍑?======
     private double lastVx = 0.0;
     private double lastVy = 0.0;
     private double lastOmega = 0.0;
 
     private NetworkTable autoControlNetworkTable;
     
-    private final RobotStatusManager robotStatusManager;
+    private final Superstructure superstructure;
     public void setBoostEnabled(boolean enabled) {
         boostEnabled = enabled;
     }
 
-    // 新增：当前是否使用场地坐标系（默认 true）
+    // 鏂板锛氬綋鍓嶆槸鍚︿娇鐢ㄥ満鍦板潗鏍囩郴锛堥粯璁?true锛?
     private boolean fieldCentricEnabled = true;
 
     private final SwerveRequest.FieldCentric fieldCentric =
@@ -86,10 +86,10 @@ public class DriveControls {
         .withRotationalDeadband(maxAngularRate * 0.05)
         .withDriveRequestType(DriveRequestType.Velocity);
     
-    public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
+    public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, Superstructure superstructure) {
         this.drivetrain = drivetrain;
         this.driver = driver;
-        this.robotStatusManager = rsm;
+        this.superstructure = superstructure;
         autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
     }
     
@@ -101,16 +101,16 @@ public class DriveControls {
         return fieldCentricEnabled;
     }
 
-    /** 底盘的默认驾驶命令（teleop 期间持续执行） */
+    /** 搴曣洏鐨勯粯璁ら┚椹跺懡浠わ紙teleop 鏈熼棿鎸佺画鎵ц锛?*/
     public Command defaultDriveCommand() {
         return drivetrain.applyRequest(() -> {
-        SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
+        WantedState wantedState = superstructure.getWantedState();
+        SmartDashboard.putString("wantedState", wantedState.name());
         double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
         double turnScale  = boostEnabled ? boostTurnScale  : normalTurnScale;
         double vx = -driver.getLeftY() * maxSpeed * driveScale;
         double vy = -driver.getLeftX() * maxSpeed * driveScale;
         double vomega = -driver.getRightX() * maxAngularRate * turnScale;
-        // ====== execute 内替换 limiter：只限加速、不限减速 ======
         double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
         double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
         double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter);
@@ -123,105 +123,40 @@ public class DriveControls {
         vy = limitedVy;
         vomega = limitedOmega;
 
-        double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+        double[] hubAim = calculateDistanceAndRotationToHub();
-        // 将距离值写入 NetworkTable
+        double[] passAim = calculateDistanceAndRotationToPassBall();
-        autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
+        autoControlNetworkTable.getEntry("distanceToHub").setDouble(hubAim[0]);
-        autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
+        autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(hubAim[1]);
-        autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
+        autoControlNetworkTable.getEntry("distanceToPassball").setDouble(passAim[0]);
-        autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
+        autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(passAim[1]);
-        // 把朝向 hub 的目标角同步给底盘，供 Superstructure 读取 isAimed() 做射击门控；
-        // 数据来源与 AutoAimming 自动旋转完全相同，保证门控和实际瞄准一致。
-        drivetrain.setTargetHeading(
-            drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
 
-        switch (robotStatusManager.getStatus()) {
+        double[] activeAim = wantedState == WantedState.PASS_BALL ? passAim : hubAim;
-            case Stopped:
+        drivetrain.setTargetHeading(
-                vx = 0;
+            drivetrain.getRotation().plus(Rotation2d.fromDegrees(activeAim[1])));
-                vy = 0;
+
-                vomega = 0;
+        switch (wantedState) {
-                break;
+            case SHOOT_HUB:
-            case PassingBall:
+            case AIM_HUB:
-                //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+                vomega = calculateRotationSpeedFromRotationAngle(hubAim[1]);
-                vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
-                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
-                break;
-            case AutoAimming:
-                //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
-                vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
                 vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
                 vy = -driver.getLeftX() * maxSpeed * slowDriveScale;
                 break;
-            case AllTelop:
+            case PASS_BALL:
+                vomega = calculateRotationSpeedFromRotationAngle(passAim[1]);
+                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
+                break;
+            default:
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
-            case CrossingBump:
-                double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
-                if (Math.abs(differenceBump) < 0.1) {
-                    vy = 0;
-                } else {
-                    double raw = differenceBump * Constants.AutoPositioning.autoPositioningkP;
-                    vy = Math.copySign(
-                        Math.max(Math.abs(raw), 0.4),
-                        raw
-                    );
-                    vy = isRedAlliance ? -vy : vy;
-                }
-                autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
-
-                Rotation2d BumpcurrentAngle = drivetrain.getState().Pose.getRotation();
-                double BumpcurrentRad = BumpcurrentAngle.getRadians();
-                double targetA = Math.toRadians(45);
-                double targetB = Math.toRadians(-45);
-                // 计算误差
-                double errorA = targetA - BumpcurrentRad;
-                double errorB = targetB - BumpcurrentRad;
-                // 角度归一化到 [-pi, pi]
-                errorA = Math.atan2(Math.sin(errorA), Math.cos(errorA));
-                errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
-                // 选误差绝对值更小的那个
-                double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
-                vomega = calculatevomegaFromRotationAngle(chosenError);
-                autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega);
-                //vomega = calculateRotationSpeedForBump();
-                break;
-            case CrossingTrench:
-                double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]);
-                if (Math.abs(differenceTrench) < 0.1) {
-                    vy = 0;
-                } else {
-                    double raw = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
-                    vy = Math.copySign(
-                        Math.max(Math.abs(raw), 0.4),
-                        raw
-                    );
-                    vy = isRedAlliance ? -vy : vy;
-                }
-                autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
-                Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
-                double currentRad = currentAngle.getRadians();
-                double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
-                double TrenchAngleerror = targetRad - currentRad;
-                vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
-                autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega);
-                //vomega = calculateRotationSpeedForTrench();
-                break;
         }
 
         autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
         autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
         autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
 
-        boolean isAutoLike =
+        if (fieldCentricEnabled) {
-            robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
+        return fieldCentric
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
-
-        if (fieldCentricEnabled || isAutoLike) {
-
-    // 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
-        //var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
-        var chosen = fieldCentric;
-        return chosen
                 .withVelocityX(vx)
                 .withVelocityY(vy)
                 .withRotationalRate(vomega);
@@ -231,16 +166,12 @@ public class DriveControls {
                 .withVelocityY(vy)
                 .withRotationalRate(vomega);
         }
-        }, () -> fieldCentricEnabled || 
+        }, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
-            robotStatusManager.getStatus() == RobotStatus.CrossingBump || 
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench
-             ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
     }
-
     /**
      * 
-         * 输出移动到距离两个目标之间较近的那个点的位移
+         * 杈撳嚭绉诲姩鍒拌窛绂讳袱涓洰鏍囦箣闂磋緝杩戠殑閭ｄ釜鐐圭殑浣嶇Щ
-         * @return 位移 有正负
+         * @return 浣嶇Щ 鏈夋璐?
          */
     public double calculateDifferenceToTwoTarget(double current, double target1, double target2){
           double x1 = target1 - current;
@@ -248,16 +179,16 @@ public class DriveControls {
           return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
     }
     public double[] calculateDistanceAndRotationToHub() {
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
+        Pose2d currentPose = drivetrain.getState().Pose; // 鑾峰彇鏈哄櫒浜哄綋鍓嶇殑浣嶇疆鍜岃搴?
         double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
         double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY;
 
-        // 计算目标角度（相对于场地坐标系）
+        // 璁＄畻鐩爣瑙掑害锛堢浉瀵逛簬鍦哄湴鍧愭爣绯伙級
         double deltaX = targetX - currentPose.getX();
         double deltaY = targetY - currentPose.getY();
-        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
+        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 杞崲涓哄害
 
-        // 当前机器人角度
+        // 褰撳墠鏈哄櫒浜鸿搴?
         double currentAngle = currentPose.getRotation().getDegrees();
         double angleDifference = targetAngle - currentAngle;
         // Normalize angle difference to the range [-180, 180]
@@ -267,26 +198,26 @@ public class DriveControls {
             angleDifference += 360;
         }
 
-        // 计算距离
+        // 璁＄畻璺濈
         double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
-                // 计算距离和旋转角度
+                // 璁＄畻璺濈鍜屾棆杞搴?
         double[] distanceAndRotation = new double[]{distance, angleDifference};
 
-        // 返回距离和角度差
+        // 杩斿洖璺濈鍜岃搴﹀樊
         return distanceAndRotation;
     }
     public double[] calculateDistanceAndRotationToPassBall() {
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
+        Pose2d currentPose = drivetrain.getState().Pose; // 鑾峰彇鏈哄櫒浜哄綋鍓嶇殑浣嶇疆鍜岃搴?
-        double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX;  // 目标 X 坐标
+        double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX;  // 鐩爣 X 鍧愭爣
-        double targetY1 = Constants.Field.PassingBallPosY1;  // 目标 Y 坐标
+        double targetY1 = Constants.Field.PassingBallPosY1;  // 鐩爣 Y 鍧愭爣
-        double targetY2 = Constants.Field.PassingBallPosY2;  // 目标 Y 坐标
+        double targetY2 = Constants.Field.PassingBallPosY2;  // 鐩爣 Y 鍧愭爣
        
-        // 计算目标角度（相对于场地坐标系）
+        // 璁＄畻鐩爣瑙掑害锛堢浉瀵逛簬鍦哄湴鍧愭爣绯伙級
         double deltaX = targetX - currentPose.getX();
         double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
-        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
+        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 杞崲涓哄害
 
-        // 当前机器人角度
+        // 褰撳墠鏈哄櫒浜鸿搴?
         double currentAngle = currentPose.getRotation().getDegrees();
         double angleDifference = targetAngle - currentAngle;
         // Normalize angle difference to the range [-180, 180]
@@ -296,12 +227,12 @@ public class DriveControls {
             angleDifference += 360;
         }
 
-        // 计算距离
+        // 璁＄畻璺濈
         double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
-                // 计算距离和旋转角度
+                // 璁＄畻璺濈鍜屾棆杞搴?
         double[] distanceAndRotation = new double[]{distance, angleDifference};
 
-        // 返回距离和角度差
+        // 杩斿洖璺濈鍜岃搴﹀樊
         return distanceAndRotation;
     }
 
@@ -329,26 +260,26 @@ public class DriveControls {
             } else {
                 vomega = error * Constants.AutoPositioning.TurningkP;
             }
-            // 限制最大角速度
+            // 闄愬埗鏈€澶ц閫熷害
             vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
         return vomega;
     }
 
         /**
-     * 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
+     * 鍙檺鍒垛€滃姞閫熲€濓紙骞呭€煎彉澶э級锛屼笉闄愬埗鈥滃噺閫熲€濓紙骞呭€煎彉灏忥級
-     * 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
+     * 杩欐牱鏉炬潌/鍒硅溅浼氭洿鍙婃椂锛屽悓鏃?boost 浠嶇劧鑳芥姂鍒剁數娴佸啿鍑?
      */
     private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
-        // 如果正在减速（幅值变小），直接放行到目标值
+        // 濡傛灉姝ｅ湪鍑忛€燂紙骞呭€煎彉灏忥級锛岀洿鎺ユ斁琛屽埌鐩爣鍊?
         if (Math.abs(target) < Math.abs(last)) {
-        limiter.reset(target);  // 同步 limiter 内部状态，防止下一次突变
+        limiter.reset(target);  // 鍚屾 limiter 鍐呴儴鐘舵€侊紝闃叉涓嬩竴娆＄獊鍙?
         return target;
         }
-        // 否则是加速，使用 limiter
+        // 鍚﹀垯鏄姞閫燂紝浣跨敤 limiter
         return limiter.calculate(target);
     }
 
-        /** 紧急停止：清零限速器，让速度输出立刻归零 */
+        /** 绱ф€ュ仠姝細娓呴浂闄愰€熷櫒锛岃閫熷害杈撳嚭绔嬪埢褰掗浂 */
     public void emergencyStop() {
         vxLimiter.reset(0.0);
         vyLimiter.reset(0.0);
@@ -386,13 +317,13 @@ public class DriveControls {
         return maxAngularRate;
     }
     public void setTeamColors() {
-        Optional<Alliance> ally = DriverStation.getAlliance(); // 获取队伍颜色
+        Optional<Alliance> ally = DriverStation.getAlliance(); // 鑾峰彇闃熶紞棰滆壊
         if (ally.isPresent()) {
-            // 根据队伍颜色设置布尔值
+            // 鏍规嵁闃熶紞棰滆壊璁剧疆甯冨皵鍊?
             if (ally.get() == Alliance.Red) {
-                isRedAlliance = true; // 红队
+                isRedAlliance = true; // 绾㈤槦
             } else if (ally.get() == Alliance.Blue) {
-                isRedAlliance = false; // 蓝队
+                isRedAlliance = false; // 钃濋槦
             }
         } else {
             isRedAlliance = true;
@@ -438,20 +369,20 @@ public class DriveControls {
 
             switch (phase) {
                 case 0:
-                    vx = amplitude;   // 前
+                    vx = amplitude;   // 鍓?
                     vy = 0.0;
                     break;
                 case 1:
                     vx = 0.0;
-                    vy = amplitude;   // 右
+                    vy = amplitude;   // 鍙?
                     break;
                 case 2:
-                    vx = -amplitude;  // 后
+                    vx = -amplitude;  // 鍚?
                     vy = 0.0;
                     break;
                 case 3:
                     vx = 0.0;
-                    vy = -amplitude;  // 左
+                    vy = -amplitude;  // 宸?
                     break;
             }
 
@@ -473,11 +404,11 @@ public class DriveControls {
 
     public void pushDistanceData(){
         double[] distanceAndRotation = calculateDistanceAndRotationToHub();
-        // 将距离值写入 NetworkTable
+        // 灏嗚窛绂诲€煎啓鍏?NetworkTable
         autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
         autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
-        // 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新，
+        // 鑷姩闃舵榛樿椹鹃┒鍛戒护涓嶈繍琛岋紝杩欓噷鍚屾牱淇濇寔鐩爣鏈濆悜鏈€鏂帮紝
-        // 让自动程序里的射击门控 isAimed() 有效。
+        // 璁╄嚜鍔ㄧ▼搴忛噷鐨勫皠鍑婚棬鎺?isAimed() 鏈夋晥銆?
         drivetrain.setTargetHeading(
             drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
     }

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

@@ -2,6 +2,7 @@ package frc.robot.controls;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
 import frc.robot.subsystems.IntakeRotaterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.Superstructure;
@@ -29,12 +30,16 @@ public class OperatorControls {
         return superstructure.requestIntakeCommand();
     }
 
-    public Command requestPrepShootCommand() {
+    public Command requestShootHubCommand() {
-        return superstructure.requestPrepShootCommand();
+        return superstructure.requestShootHubCommand();
     }
 
-    public Command requestShootCommand() {
+    public Command requestAimHubCommand() {
-        return superstructure.requestShootCommand();
+        return superstructure.requestAimHubCommand();
+    }
+
+    public Command requestPassBallCommand() {
+        return superstructure.requestPassBallCommand();
     }
 
     public Command requestEjectCommand() {
@@ -73,10 +78,8 @@ public class OperatorControls {
     // 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。
     public Command autoShootToHubCommand() {
         return Commands.sequence(
-            superstructure.requestPrepShootCommand(),
+            superstructure.requestShootHubCommand(),
-            Commands.waitUntil(superstructure::canShoot).withTimeout(1.5),
+            Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
-            superstructure.requestShootCommand(),
-            Commands.waitSeconds(1.2),
             superstructure.requestIdleCommand()
         );
     }

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

@@ -23,8 +23,9 @@ public class Superstructure extends SubsystemBase {
     public enum WantedState {
         IDLE,
         INTAKE,
-        PREP_SHOOT,
+        SHOOT_HUB,
-        SHOOT,
+        AIM_HUB,
+        PASS_BALL,
         EJECT,
         MANUAL
     }
@@ -34,11 +35,8 @@ public class Superstructure extends SubsystemBase {
         DISABLED,
         STOWED,
         INTAKING,
-        SPINNING_UP,
+        PREP_SHOOT,
-        AIMING,
-        READY_TO_SHOOT,
         SHOOTING,
-        CLEANUP,
         EJECTING,
         MANUAL_OVERRIDE
     }
@@ -56,6 +54,7 @@ public class Superstructure extends SubsystemBase {
     private double stateStartTimestamp = Timer.getFPGATimestamp();
     private double flywheelSpeedOffsetRps = 0.0;
     private double backboardPositionOffset = 0.0;
+    private boolean shotCompletedThisRequest = false;
 
     public Superstructure(
         ShooterSubsystem shooter,
@@ -85,12 +84,26 @@ public class Superstructure extends SubsystemBase {
         wantedState = WantedState.INTAKE;
     }
 
-    public void requestPrepShoot() {
+    public void requestShootHub() {
-        wantedState = WantedState.PREP_SHOOT;
+        if (wantedState != WantedState.SHOOT_HUB) {
+            shotCompletedThisRequest = false;
+        }
+        wantedState = WantedState.SHOOT_HUB;
+    }
+
+    public void requestAimHub() {
+        wantedState = WantedState.AIM_HUB;
+    }
+
+    public void requestPassBall() {
+        if (wantedState != WantedState.PASS_BALL) {
+            shotCompletedThisRequest = false;
+        }
+        wantedState = WantedState.PASS_BALL;
     }
 
     public void requestShoot() {
-        wantedState = WantedState.SHOOT;
+        requestShootHub();
     }
 
     public void requestEject() {
@@ -109,12 +122,20 @@ public class Superstructure extends SubsystemBase {
         return Commands.runOnce(this::requestIntake, this);
     }
 
-    public Command requestPrepShootCommand() {
+    public Command requestShootHubCommand() {
-        return Commands.runOnce(this::requestPrepShoot, this);
+        return Commands.runOnce(this::requestShootHub, this);
+    }
+
+    public Command requestAimHubCommand() {
+        return Commands.runOnce(this::requestAimHub, this);
+    }
+
+    public Command requestPassBallCommand() {
+        return Commands.runOnce(this::requestPassBall, this);
     }
 
     public Command requestShootCommand() {
-        return Commands.runOnce(this::requestShoot, this);
+        return requestShootHubCommand();
     }
 
     public Command requestEjectCommand() {
@@ -133,6 +154,10 @@ public class Superstructure extends SubsystemBase {
         return systemState;
     }
 
+    public boolean isShootCycleComplete() {
+        return shotCompletedThisRequest;
+    }
+
     /* ====================== */
     /*        射击门控          */
     /* ====================== */
@@ -188,8 +213,9 @@ public class Superstructure extends SubsystemBase {
         switch (wantedState) {
             case IDLE -> handleIdle();
             case INTAKE -> handleIntake();
-            case PREP_SHOOT -> handlePrepShoot(false);
+            case SHOOT_HUB -> handleShootHub();
-            case SHOOT -> handlePrepShoot(true);
+            case AIM_HUB -> handleAimHub();
+            case PASS_BALL -> handlePassBall();
             case EJECT -> handleEject();
             case MANUAL -> handleManual();
         }
@@ -204,6 +230,7 @@ public class Superstructure extends SubsystemBase {
         shooter.stopFlywheel();
         shooter.stopShooterConveyor();
         shooter.holdBackboardAt(0.0);
+        setSystemState(SystemState.STOWED);
     }
 
     private void handleIntake() {
@@ -218,24 +245,39 @@ public class Superstructure extends SubsystemBase {
         setSystemState(SystemState.INTAKING);
     }
 
-    private void handlePrepShoot(boolean feedWhenReady) {
+    private void handleAimHub() {
-        if (systemState != SystemState.SPINNING_UP
+        intakeRoller.stop();
-            && systemState != SystemState.AIMING
+        conveyor.stop();
-            && systemState != SystemState.READY_TO_SHOOT
+        shooter.stopFlywheel();
-            && systemState != SystemState.SHOOTING
+        shooter.stopShooterConveyor();
-            && systemState != SystemState.CLEANUP) {
+        shooter.holdBackboardAt(0.0);
-            setSystemState(SystemState.SPINNING_UP);
+        setSystemState(SystemState.STOWED);
-        }
+    }
 
-        intakeRotater.lowerForMatch();
+    private void handleShootHub() {
+        double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
+        double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
+        handlePreparedFeed(targetRps, backboardPosition);
+    }
+
+    private void handlePassBall() {
+        double targetRps = Constants.Superstructure.passBallFlywheelRps + flywheelSpeedOffsetRps;
+        double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
+        handlePreparedFeed(targetRps, backboardPosition);
+    }
+
+    private void handlePreparedFeed(double targetRps, double backboardPosition) {
+        if (systemState != SystemState.PREP_SHOOT
+            && systemState != SystemState.SHOOTING) {
+            setSystemState(SystemState.PREP_SHOOT);
+        }
         intakeRoller.stop();
         conveyor.stop();
         shooter.stopShooterConveyor();
         // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
         // 但不会绕过 Superstructure 状态机。
-        double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
         shooter.setFlywheelVelocity(targetRps);
-        shooter.holdBackboardAt(Constants.Superstructure.backboardShootPosition + backboardPositionOffset);
+        shooter.holdBackboardAt(backboardPosition);
 
         if (systemState == SystemState.SHOOTING) {
             // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
@@ -245,17 +287,8 @@ public class Superstructure extends SubsystemBase {
             shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
 
             if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
-                setSystemState(SystemState.CLEANUP);
+                shotCompletedThisRequest = true;
-            }
+                setSystemState(SystemState.PREP_SHOOT);
-            return;
-        }
-
-        if (systemState == SystemState.CLEANUP) {
-            intakeRotater.lowerForMatch();
-            conveyor.stop();
-            shooter.stopShooterConveyor();
-            if (timeInState() > Constants.Superstructure.cleanupSeconds) {
-                setSystemState(SystemState.STOWED);
             }
             return;
         }
@@ -266,14 +299,6 @@ public class Superstructure extends SubsystemBase {
 
         // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
         if (canShoot()) {
-            setSystemState(SystemState.READY_TO_SHOOT);
-        } else if (shooter.atSpeed()) {
-            setSystemState(SystemState.AIMING);
-        } else {
-            setSystemState(SystemState.SPINNING_UP);
-        }
-
-        if (feedWhenReady && systemState == SystemState.READY_TO_SHOOT) {
             setSystemState(SystemState.SHOOTING);
         }
     }