7.9晚

2026-07-09 20:12:25 +08:00
parent 914584b71c
commit 915b1b1af0
10 changed files with 1193 additions and 131 deletions
@@ -18,30 +18,52 @@ public final class Constants {
        public static final int rightConveyorID = 37;
        public static final int backboardMotorID = 44;

+        public static final double flywheelNominalKv = 0.12;
        public static final double defaultFlywheelVoltage = 3.34;
-        public static final double flywheelReadyVoltageThreshold = 1.0;
+        public static final double defaultFlywheelVelocity = defaultFlywheelVoltage / flywheelNominalKv;
+        public static final double flywheelMaxVelocity = 120.0;
+        public static final double flywheelReadyVelocityThreshold = 5.0;
+        public static final double flywheelReadyVelocityTolerance = 2.0;
        public static final double flywheelReadyDelaySeconds = 0.5;

        // 距离(米) -> 飞轮电压(V) 射表。当前为占位点；
        // 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。
        public static final InterpolatingDoubleTreeMap flywheelVoltageByDistance = new InterpolatingDoubleTreeMap();
        static {
-            flywheelVoltageByDistance.put(1.2, 5.82);
-            flywheelVoltageByDistance.put(2.5, 6.34);
-            flywheelVoltageByDistance.put(3.5, 6.73);
-            flywheelVoltageByDistance.put(5.0, 7.33);
+            flywheelVoltageByDistance.put(1.2, 2.0);
+            flywheelVoltageByDistance.put(2.5, 2.1);
+            flywheelVoltageByDistance.put(3.5, 2.2);
+            flywheelVoltageByDistance.put(5.0, 2.3);
+        }
+
+        public static final InterpolatingDoubleTreeMap flywheelVelocityByDistance = new InterpolatingDoubleTreeMap();
+        static {
+            flywheelVelocityByDistance.put(1.2, 2.0 / flywheelNominalKv);
+            flywheelVelocityByDistance.put(2.5, 2.1 / flywheelNominalKv);
+            flywheelVelocityByDistance.put(3.5, 2.2 / flywheelNominalKv);
+            flywheelVelocityByDistance.put(5.0, 2.3 / flywheelNominalKv);
        }

        public static final double shooterConveyorFeedVoltage = 3.0;
        public static final double shooterConveyorReverseVoltage = -2.0;
+        public static final double shooterConveyorNominalKv = 0.125;
+        public static final double shooterConveyorMaxVelocity = 12.0 / shooterConveyorNominalKv;
+        public static final double shooterConveyorFeedVelocity =
+            shooterConveyorFeedVoltage / shooterConveyorNominalKv;
+        public static final double shooterConveyorReverseVelocity =
+            shooterConveyorReverseVoltage / shooterConveyorNominalKv;

        public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
-            .withKP(0.0).withKI(0).withKD(0)
-            .withKV(0.099).withKA(0.0).withKS(0.0);
+            .withKP(0.21)
+            .withKI(0)
+            .withKD(0.004)
+            .withKS(0.25)
+            .withKA(0.0)
+            .withKV(flywheelNominalKv);

        public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
-            .withKP(0.0).withKI(0).withKD(0)
-            .withKV(0.16).withKA(0.15).withKS(0);
+            .withKP(0.19).withKI(0).withKD(0.001)
+            .withKV(shooterConveyorNominalKv).withKA(0.0).withKS(0.07);

        public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
            .withKP(0.01).withKI(0).withKD(0)
@@ -65,7 +87,7 @@ public final class Constants {
    }

    public static class AutoPositioning {
-        public static final double autoRotationkP = 0.08;
+        public static final double autoRotationkP = 0.07;
        public static final double autoRotationForBumpTargetDegrees = 45;
        public static final double autoRotationForTrenchTargetDegrees = 0;
        public static final boolean teleopOffsetEnabled = true;
@@ -130,9 +152,6 @@ public final class Constants {
        public static final double intakeRotaterRaiseVoltage = 0.8;
        public static final double intakeRotaterLowerVoltage = 0.05;

-        public static final double intakeRotaterShootAssistPeriodSeconds = 0.4;
-        public static final double intakeRotaterShootAssistUpDutyCycle = 0.5;
-
        public static final Slot0Configs intakeRollerSlot0Configs = new Slot0Configs()
            .withKP(0.00).withKI(0.00).withKD(0.00)
            .withKV(0.00).withKA(0.00).withKS(0.00);
@@ -160,14 +179,18 @@ public final class Constants {
        // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
        public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage;
        public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage;
+        public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity;
+        public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity;
        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 passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage;
+        public static final double passBallFlywheelVelocity = Shooter.defaultFlywheelVelocity;
        public static final double passBallBackboardPosition = 175.0;

        // 射击时背板的目标位置（外接编码器计数）。
        public static final double backboardShootPosition = 175.0;
+        public static final double backboardReadyPositionTolerance = 100.0;
        // 射击门控是否要求底盘接近静止。
        public static final boolean stationaryGateEnabled = true;
    }
@@ -181,8 +181,8 @@ public class RobotContainer {
    controllerupper.b().and(teleopOnly)
        .onTrue(operatorControls.requestEjectCommand())
        .onFalse(operatorControls.requestIdleCommand());
-    controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(-0.1)));
-    controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(0.1)));
+    controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0)));
+    controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0)));
    controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
    controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
    controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets()));
@@ -200,12 +200,15 @@ public class RobotContainer {
  public void tunerPeriodic() {
    driveGainsTuner.periodic();
    configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
+    shooterSubsystem.debugFlywheelVelocityPeriodic();
    configConveyorTuner.periodic(null);
+    shooterSubsystem.debugShooterConveyorVelocityPeriodic();
    configBackboardTuner.periodic();
    shooterSubsystem.debugBackboardTargetPeriodic();
    configMainConveyorTuner.periodic(null);
    configIntakerRollerTuner.periodic(null);
    configIntakerRotaterTuner.periodic(null);
+    shooterSubsystem.setRightFlywheelFollowLeft();
    shooterSubsystem.setRightConveyorFollowLeft();
  }

@@ -244,7 +244,11 @@ public class DriveControls {

        // If angle difference is within threshold, don't rotate
        if (Math.abs(calDifference) > 1.0) {
-            return calDifference  * Constants.AutoPositioning.autoRotationkP;
+            double rotationRate = calDifference * Constants.AutoPositioning.autoRotationkP;
+            if (Math.abs(rotationRate) < 0.24) {
+                return Math.copySign(0.24, rotationRate);
+            }
+            return rotationRate;
        } else {
            return 0;
        }
@@ -54,6 +54,10 @@ public class OperatorControls {
        superstructure.adjustFlywheelVoltageOffset(offset);
    }

+    public void adjustFlywheelVelocityOffset(double offset) {
+        superstructure.adjustFlywheelVelocityOffset(offset);
+    }
+
    public void adjustBackboardRateOffset(double offset) {
        superstructure.adjustBackboardPositionOffset(offset);
    }
@@ -69,12 +69,20 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
    }

    public void shootAssist() {
-        double period = Constants.Intaker.intakeRotaterShootAssistPeriodSeconds;
-        double phase = (Timer.getFPGATimestamp() % period) / period;
-        double volts = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle
-            ? Constants.Intaker.intakeRotaterRaiseVoltage
-            : Constants.Intaker.intakeRotaterLowerVoltage;
-        setVoltage(volts);
+        double cycleTime = Timer.getFPGATimestamp() % 1.7;
+
+        if (cycleTime < 0.7) {
+            setVoltage(1);
+        } else if (cycleTime < 0.9) {
+            setVoltage(0.0);
+        } else {
+            setVoltage(0.03);
+        }
+    }
+
+    public void postShootIdleAssist(double elapsedSeconds) {
+        double cycleTime = elapsedSeconds % 0.2;
+        setVoltage(cycleTime < 0.1 ? 0.03 : 0.0);
    }

    public void manualRaise() {
@@ -51,4 +51,8 @@ public class LimelightSubsystem extends SubsystemBase {
        // 查 Constants 里的插值射表；真车试射后往表里加实测点即可。
        return Constants.Shooter.flywheelVoltageByDistance.get(getDistanceToTarget());
    }
+
+    public double getShooterVelocityByDistance() {
+        return Constants.Shooter.flywheelVelocityByDistance.get(getDistanceToTarget());
+    }
 }
@@ -3,6 +3,7 @@ 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.VelocityVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.hardware.TalonFXS;
@@ -39,18 +40,25 @@ public class ShooterSubsystem extends SubsystemBase {
    public final Encoder backboardEncoder;
    public final ProfiledPIDController backboardPID;

-    private final VoltageOut leftFlywheelVoltageRequest = new VoltageOut(0);
-    private final VoltageOut rightFlywheelVoltageRequest = new VoltageOut(0);
-    private final VoltageOut conveyorVoltageRequest = new VoltageOut(0);
+    private final VelocityVoltage leftFlywheelVelocityRequest = new VelocityVoltage(0).withSlot(0);
+    private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0).withSlot(0);
    private final VoltageOut backboardVoltageRequest = new VoltageOut(0);

+    private final Follower flywheelFollower;
    private final Follower conveyorFollower;

    private final NetworkTable shooterNetworkTable;
+    private final NetworkTable flywheelTuningTable;
+    private final NetworkTable conveyorTuningTable;
    private final NetworkTable backboardTuningTable;

    private double flywheelOutputVolts = 0.0;
-    private double flywheelVoltageStartTimestamp = 0.0;
+    private double flywheelTargetVelocity = 0.0;
+    private double flywheelVelocityTolerance = Constants.Shooter.flywheelReadyVelocityTolerance;
+    private double flywheelVelocityReadyStartTimestamp = 0.0;
+    private boolean flywheelVelocityTuningActive = false;
+    private double shooterConveyorTargetVelocity = 0.0;
+    private boolean shooterConveyorVelocityTuningActive = false;
    private double shooterConveyorOutputVolts = 0.0;
    private double backboardOutputVolts = 0.0;
    private double backboardRawPidOutputVolts = 0.0;
@@ -75,7 +83,9 @@ public class ShooterSubsystem extends SubsystemBase {
        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);
+        setRightFlywheelFollowLeft();
        setRightConveyorFollowLeft();

        backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
@@ -95,6 +105,13 @@ public class ShooterSubsystem extends SubsystemBase {
        backboardPID.setTolerance(10.0);

        shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
+        flywheelTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/flywheel");
+        flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false);
+        flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity);
+        flywheelTuningTable.getEntry("flywheelVelocityTolerance").setDefaultDouble(Constants.Shooter.flywheelReadyVelocityTolerance);
+        conveyorTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/conveyor");
+        conveyorTuningTable.getEntry("conveyorVelocityEnable").setDefaultBoolean(false);
+        conveyorTuningTable.getEntry("conveyorTargetVelocity").setDefaultDouble(Constants.Shooter.shooterConveyorFeedVelocity);
        backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
        backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
        backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
@@ -109,6 +126,10 @@ public class ShooterSubsystem extends SubsystemBase {
        rightConveyor.setControl(conveyorFollower);
    }

+    public void setRightFlywheelFollowLeft() {
+        rightFlywheel.setControl(flywheelFollower);
+    }
+
    public void applyLeftConfigurationToRight() {
        Slot0Configs configs = new Slot0Configs();
        leftFlywheel.getConfigurator().refresh(configs);
@@ -117,29 +138,51 @@ public class ShooterSubsystem extends SubsystemBase {

    public void setFlywheelVoltage(double outputVolts) {
        double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
-        if (Math.abs(clampedOutputVolts - flywheelOutputVolts) > 1e-6) {
-            flywheelVoltageStartTimestamp = Timer.getFPGATimestamp();
+        setFlywheelVelocity(clampedOutputVolts / Constants.Shooter.flywheelNominalKv);
+    }
+
+    public void setFlywheelVelocity(double targetVelocityRps) {
+        double clampedTargetVelocity = MathUtil.clamp(
+            targetVelocityRps,
+            -Constants.Shooter.flywheelMaxVelocity,
+            Constants.Shooter.flywheelMaxVelocity
+        );
+        if (Math.abs(clampedTargetVelocity - flywheelTargetVelocity) > 1e-6) {
+            flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
        }
-        flywheelOutputVolts = clampedOutputVolts;
-        leftFlywheel.setControl(leftFlywheelVoltageRequest.withOutput(flywheelOutputVolts));
-        rightFlywheel.setControl(rightFlywheelVoltageRequest.withOutput(-flywheelOutputVolts));
+        flywheelTargetVelocity = clampedTargetVelocity;
+        leftFlywheel.setControl(leftFlywheelVelocityRequest.withVelocity(flywheelTargetVelocity));
+        setRightFlywheelFollowLeft();
    }

    public void setFlywheelVoltageByDistance(double distance) {
-        double targetVolts = Constants.Shooter.flywheelVoltageByDistance.get(distance);
-        setFlywheelVoltage(targetVolts);
+        setFlywheelVelocityByDistance(distance);
+    }
+
+    public void setFlywheelVelocityByDistance(double distance) {
+        double targetVelocity = Constants.Shooter.flywheelVelocityByDistance.get(distance);
+        setFlywheelVelocity(targetVelocity);
    }

    public void stopFlywheel() {
-        setFlywheelVoltage(0.0);
+        setFlywheelVelocity(0.0);
+    }
+
+    public boolean atVelocity() {
+        if (Math.abs(flywheelTargetVelocity) < Constants.Shooter.flywheelReadyVelocityThreshold) {
+            flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
+            return false;
+        }
+        if (Math.abs(getFlywheelVelocityError()) > flywheelVelocityTolerance) {
+            flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
+            return false;
+        }
+        return Timer.getFPGATimestamp() - flywheelVelocityReadyStartTimestamp
+            >= Constants.Shooter.flywheelReadyDelaySeconds;
    }

    public boolean atVoltage() {
-        if (Math.abs(flywheelOutputVolts) < Constants.Shooter.flywheelReadyVoltageThreshold) {
-            return false;
-        }
-        return Timer.getFPGATimestamp() - flywheelVoltageStartTimestamp
-            >= Constants.Shooter.flywheelReadyDelaySeconds;
+        return atVelocity();
    }
    public double getLeftFlywheelVelocity() {
        return leftFlywheel.getVelocity().getValueAsDouble();
@@ -157,22 +200,51 @@ public class ShooterSubsystem extends SubsystemBase {
        return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
    }

+    public double getTargetFlywheelVelocity() {
+        return flywheelTargetVelocity;
+    }
+
+    public double getFlywheelVelocityError() {
+        return Math.abs(flywheelTargetVelocity) - getAverageFlywheelVelocity();
+    }
+
    public double getTargetFlywheelVoltage() {
-        return flywheelOutputVolts;
+        return flywheelTargetVelocity * Constants.Shooter.flywheelNominalKv;
    }

    /* ====================== */
    /*    shooter conveyor     */
    /* ====================== */

-    public void runShooterConveyorVoltage(double outputVolts) {
-        shooterConveyorOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
-        leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorOutputVolts));
+    public void runShooterConveyorVelocity(double targetVelocityRps) {
+        shooterConveyorTargetVelocity = MathUtil.clamp(
+            targetVelocityRps,
+            -Constants.Shooter.shooterConveyorMaxVelocity,
+            Constants.Shooter.shooterConveyorMaxVelocity
+        );
+        leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity));
        setRightConveyorFollowLeft();
    }

+    public void runShooterConveyorVoltage(double outputVolts) {
+        double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
+        runShooterConveyorVelocity(clampedOutputVolts / Constants.Shooter.shooterConveyorNominalKv);
+    }
+
    public void stopShooterConveyor() {
-        runShooterConveyorVoltage(0.0);
+        runShooterConveyorVelocity(0.0);
+    }
+
+    public double getShooterConveyorVelocity() {
+        return leftConveyor.getVelocity().getValueAsDouble();
+    }
+
+    public double getShooterConveyorTargetVelocity() {
+        return shooterConveyorTargetVelocity;
+    }
+
+    public double getShooterConveyorVelocityError() {
+        return shooterConveyorTargetVelocity - getShooterConveyorVelocity();
    }

    public double getConveyorStatorCurrent() {
@@ -280,10 +352,36 @@ public class ShooterSubsystem extends SubsystemBase {
        return backboardPID.atGoal();
    }

+    public double getBackboardGoalPosition() {
+        return backboardPID.getGoal().position;
+    }
+
+    public double getBackboardPositionError() {
+        return getBackboardGoalPosition() - getBackboardPosition();
+    }
+
+    public boolean isBackboardNearTarget(double tolerance) {
+        return Math.abs(getBackboardPositionError()) < tolerance;
+    }
+
    public boolean isBackboardPositionTuningActive() {
        return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
    }

+    public boolean isFlywheelTuningControlActive() {
+        boolean voltageTunerEnabled = flywheelTuningTable.getEntry("flywheelEnable").getBoolean(false);
+        boolean voltageTunerRunning = flywheelTuningTable.getEntry("flywheelEnableRunning").getBoolean(false);
+        boolean velocityTunerEnabled = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
+        return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled;
+    }
+
+    public boolean isShooterConveyorTuningControlActive() {
+        boolean voltageTunerEnabled = conveyorTuningTable.getEntry("conveyorEnable").getBoolean(false);
+        boolean voltageTunerRunning = conveyorTuningTable.getEntry("conveyorEnableRunning").getBoolean(false);
+        boolean velocityTunerEnabled = conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
+        return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled;
+    }
+
    public void resetbackboardencoder() {
        backboardEncoder.reset();
        backboardPID.reset(0);
@@ -329,20 +427,82 @@ public class ShooterSubsystem extends SubsystemBase {
        }
    }

+    public void debugFlywheelVelocityPeriodic() {
+        flywheelVelocityTuningActive = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
+        double requestedTargetVelocity =
+            flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity);
+        flywheelVelocityTolerance = Math.max(
+            0.0,
+            flywheelTuningTable.getEntry("flywheelVelocityTolerance")
+                .getDouble(Constants.Shooter.flywheelReadyVelocityTolerance)
+        );
+
+        double clampedTargetVelocity = MathUtil.clamp(
+            requestedTargetVelocity,
+            -Constants.Shooter.flywheelMaxVelocity,
+            Constants.Shooter.flywheelMaxVelocity
+        );
+        flywheelTuningTable.getEntry("flywheelVelocityEnableActive").setBoolean(flywheelVelocityTuningActive);
+        flywheelTuningTable.getEntry("flywheelCommandedTargetVelocity").setDouble(requestedTargetVelocity);
+        flywheelTuningTable.getEntry("flywheelClampedTargetVelocity").setDouble(clampedTargetVelocity);
+        flywheelTuningTable.getEntry("flywheelVelocityToleranceActive").setDouble(flywheelVelocityTolerance);
+
+        if (flywheelVelocityTuningActive) {
+            setFlywheelVelocity(clampedTargetVelocity);
+        }
+    }
+
+    public void debugShooterConveyorVelocityPeriodic() {
+        shooterConveyorVelocityTuningActive =
+            conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
+        double requestedTargetVelocity =
+            conveyorTuningTable.getEntry("conveyorTargetVelocity")
+                .getDouble(Constants.Shooter.shooterConveyorFeedVelocity);
+        double clampedTargetVelocity = MathUtil.clamp(
+            requestedTargetVelocity,
+            -Constants.Shooter.shooterConveyorMaxVelocity,
+            Constants.Shooter.shooterConveyorMaxVelocity
+        );
+
+        conveyorTuningTable.getEntry("conveyorVelocityEnableActive")
+            .setBoolean(shooterConveyorVelocityTuningActive);
+        conveyorTuningTable.getEntry("conveyorCommandedTargetVelocity").setDouble(requestedTargetVelocity);
+        conveyorTuningTable.getEntry("conveyorClampedTargetVelocity").setDouble(clampedTargetVelocity);
+
+        if (shooterConveyorVelocityTuningActive) {
+            runShooterConveyorVelocity(clampedTargetVelocity);
+        }
+    }
+
    @Override
    public void periodic() {
        double backboardPosition = getBackboardPosition();
        double backboardGoalPosition = backboardPID.getGoal().position;
        double backboardSetpointPosition = backboardPID.getSetpoint().position;
+        flywheelOutputVolts = leftFlywheel.getMotorVoltage().getValueAsDouble();
        shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
        shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
        shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity());
        shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference());
+        shooterNetworkTable.getEntry("flywheelTargetVelocity").setDouble(getTargetFlywheelVelocity());
+        shooterNetworkTable.getEntry("flywheelVelocityError").setDouble(getFlywheelVelocityError());
+        shooterNetworkTable.getEntry("flywheelVelocityTolerance").setDouble(flywheelVelocityTolerance);
+        shooterNetworkTable.getEntry("flywheelAtVelocity").setBoolean(atVelocity());
+        shooterNetworkTable.getEntry("flywheelVelocityTuningActive").setBoolean(flywheelVelocityTuningActive);
+        shooterNetworkTable.getEntry("flywheelFollowerOpposed").setBoolean(true);
        shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage());
        shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts);
        shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage());
+        shooterNetworkTable.getEntry("leftFlywheelOutputVolts").setDouble(leftFlywheel.getMotorVoltage().getValueAsDouble());
+        shooterNetworkTable.getEntry("rightFlywheelOutputVolts").setDouble(rightFlywheel.getMotorVoltage().getValueAsDouble());
+        shooterNetworkTable.getEntry("leftFlywheelStatorCurrent").setDouble(leftFlywheel.getStatorCurrent().getValueAsDouble());
+        shooterNetworkTable.getEntry("rightFlywheelStatorCurrent").setDouble(rightFlywheel.getStatorCurrent().getValueAsDouble());
+        shooterConveyorOutputVolts = leftConveyor.getMotorVoltage().getValueAsDouble();
        shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts);
-        shooterNetworkTable.getEntry("conveyorVelocity").setDouble(leftConveyor.getVelocity().getValueAsDouble());
+        shooterNetworkTable.getEntry("conveyorTargetVelocity").setDouble(getShooterConveyorTargetVelocity());
+        shooterNetworkTable.getEntry("conveyorVelocity").setDouble(getShooterConveyorVelocity());
+        shooterNetworkTable.getEntry("conveyorVelocityError").setDouble(getShooterConveyorVelocityError());
+        shooterNetworkTable.getEntry("conveyorVelocityTuningActive").setBoolean(shooterConveyorVelocityTuningActive);
        shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
        shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
        shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);
@@ -52,9 +52,10 @@ public class Superstructure extends SubsystemBase {
    private WantedState wantedState = WantedState.IDLE;
    private SystemState systemState = SystemState.STOWED;
    private double stateStartTimestamp = Timer.getFPGATimestamp();
-    private double flywheelVoltageOffset = 0.0;
+    private double flywheelVelocityOffset = 0.0;
    private double backboardPositionOffset = 0.0;
    private boolean shotCompletedThisRequest = false;
+    private double postShootIdleAssistStartTimestamp = Double.NaN;

    public Superstructure(
        ShooterSubsystem shooter,
@@ -77,6 +78,9 @@ public class Superstructure extends SubsystemBase {
    /* ====================== */

    public void requestIdle() {
+        if (systemState == SystemState.SHOOTING) {
+            postShootIdleAssistStartTimestamp = Timer.getFPGATimestamp();
+        }
        wantedState = WantedState.IDLE;
    }

@@ -164,9 +168,10 @@ public class Superstructure extends SubsystemBase {

    public boolean canShoot() {
        boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary();
-        return shooter.atVoltage()
+        return shooter.atVelocity()
            && drive.isAimed()
-            && stationaryGate;
+            && stationaryGate
+            && isBackboardReady();
    }

    private boolean isDriveStationary() {
@@ -175,12 +180,20 @@ public class Superstructure extends SubsystemBase {
        return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond;
    }

+    private boolean isBackboardReady() {
+        return shooter.isBackboardNearTarget(Constants.Superstructure.backboardReadyPositionTolerance);
+    }
+
    /* ====================== */
    /*      现场微调 offset     */
    /* ====================== */

+    public void adjustFlywheelVelocityOffset(double offsetVelocity) {
+        flywheelVelocityOffset += offsetVelocity;
+    }
+
    public void adjustFlywheelVoltageOffset(double offsetVolts) {
-        flywheelVoltageOffset += offsetVolts;
+        adjustFlywheelVelocityOffset(offsetVolts / Constants.Shooter.flywheelNominalKv);
    }

    public void adjustBackboardPositionOffset(double offset) {
@@ -188,7 +201,7 @@ public class Superstructure extends SubsystemBase {
    }

    public void resetOffsets() {
-        flywheelVoltageOffset = 0.0;
+        flywheelVelocityOffset = 0.0;
        backboardPositionOffset = 0.0;
    }

@@ -227,16 +240,33 @@ public class Superstructure extends SubsystemBase {
        // 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。
        conveyor.stop();
        intakeRoller.stop();
-        intakeRotater.stop();
-        shooter.stopFlywheel();
-        shooter.stopShooterConveyor();
+        if (!runPostShootIdleAssist()) {
+            intakeRotater.stop();
+        }
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
        holdBackboardStowedUnlessTuning();
        setSystemState(SystemState.STOWED);
    }

+    private boolean runPostShootIdleAssist() {
+        if (Double.isNaN(postShootIdleAssistStartTimestamp)) {
+            return false;
+        }
+
+        double elapsed = Timer.getFPGATimestamp() - postShootIdleAssistStartTimestamp;
+        if (elapsed >= 0.6) {
+            postShootIdleAssistStartTimestamp = Double.NaN;
+            return false;
+        }
+
+        intakeRotater.postShootIdleAssist(elapsed);
+        return true;
+    }
+
    private void handleIntake() {
-        shooter.stopFlywheel();
-        shooter.stopShooterConveyor();
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
        holdBackboardStowedUnlessTuning();
        // Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。
        intakeRoller.intake();
@@ -247,35 +277,35 @@ public class Superstructure extends SubsystemBase {
    private void handleAimHub() {
        intakeRoller.stop();
        conveyor.stop();
-        shooter.stopFlywheel();
-        shooter.stopShooterConveyor();
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
        holdBackboardStowedUnlessTuning();
        setSystemState(SystemState.STOWED);
    }

    private void handleShootHub() {
-        double targetVolts = limelight.getShooterVoltageByDistance() + flywheelVoltageOffset;
+        double targetVelocity = limelight.getShooterVelocityByDistance() + flywheelVelocityOffset;
        double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
-        handlePreparedFeed(targetVolts, backboardPosition);
+        handlePreparedFeed(targetVelocity, backboardPosition);
    }

    private void handlePassBall() {
-        double targetVolts = Constants.Superstructure.passBallFlywheelVoltage + flywheelVoltageOffset;
+        double targetVelocity = Constants.Superstructure.passBallFlywheelVelocity + flywheelVelocityOffset;
        double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
-        handlePreparedFeed(targetVolts, backboardPosition);
+        handlePreparedFeed(targetVelocity, backboardPosition);
    }

-    private void handlePreparedFeed(double targetVolts, double backboardPosition) {
+    private void handlePreparedFeed(double targetVelocity, double backboardPosition) {
        if (systemState != SystemState.PREP_SHOOT
            && systemState != SystemState.SHOOTING) {
            setSystemState(SystemState.PREP_SHOOT);
        }
        intakeRoller.stop();
        conveyor.stop();
-        shooter.stopShooterConveyor();
+        stopShooterConveyorUnlessTuning();
        // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
        // 但不会绕过 Superstructure 状态机。
-        shooter.setFlywheelVoltage(targetVolts);
+        shooter.setFlywheelVelocity(targetVelocity);
        shooter.holdBackboardAt(backboardPosition);

        if (systemState == SystemState.SHOOTING) {
@@ -283,18 +313,13 @@ public class Superstructure extends SubsystemBase {
            // 避免球接触飞轮导致的掉速中断喂球。
            intakeRotater.shootAssist();
            conveyor.feedToShooter();
-            shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterFeedVoltage);
-
-            if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
-                shotCompletedThisRequest = true;
-                setSystemState(SystemState.PREP_SHOOT);
-            }
+            runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity);
            return;
        }

        // 未开始喂球前，球路保持静止。
        conveyor.stop();
-        shooter.stopShooterConveyor();
+        stopShooterConveyorUnlessTuning();

        // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
        if (canShoot()) {
@@ -307,8 +332,8 @@ public class Superstructure extends SubsystemBase {
        setSystemState(SystemState.EJECTING);
        intakeRoller.outtake();
        conveyor.reverse();
-        shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage);
-        shooter.stopFlywheel();
+        runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterReverseVelocity);
+        stopFlywheelUnlessTuning();
        holdBackboardStowedUnlessTuning();
    }

@@ -318,12 +343,30 @@ public class Superstructure extends SubsystemBase {
        }
    }

+    private void stopFlywheelUnlessTuning() {
+        if (!shooter.isFlywheelTuningControlActive()) {
+            shooter.stopFlywheel();
+        }
+    }
+
+    private void runShooterConveyorVelocityUnlessTuning(double targetVelocity) {
+        if (!shooter.isShooterConveyorTuningControlActive()) {
+            shooter.runShooterConveyorVelocity(targetVelocity);
+        }
+    }
+
+    private void stopShooterConveyorUnlessTuning() {
+        if (!shooter.isShooterConveyorTuningControlActive()) {
+            shooter.stopShooterConveyor();
+        }
+    }
+
    private void handleManual() {
        // 手动模式：状态机不再驱动机构，交给现场手动/调参工具；
        // 只保持飞轮与喂球停止，避免意外射出。
        setSystemState(SystemState.MANUAL_OVERRIDE);
-        shooter.stopFlywheel();
-        shooter.stopShooterConveyor();
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
        conveyor.stop();
        intakeRoller.stop();
    }
@@ -352,11 +395,19 @@ public class Superstructure extends SubsystemBase {
        table.getEntry("systemState").setString(systemState.name());
        table.getEntry("timeInState").setDouble(timeInState());
        table.getEntry("canShoot").setBoolean(canShoot());
+        table.getEntry("shooterAtVelocity").setBoolean(shooter.atVelocity());
        table.getEntry("shooterAtVoltage").setBoolean(shooter.atVoltage());
        table.getEntry("driveAimed").setBoolean(drive.isAimed());
        table.getEntry("driveHeadingError").setDouble(drive.getHeadingError());
        table.getEntry("driveStationary").setBoolean(isDriveStationary());
-        table.getEntry("flywheelVoltageOffset").setDouble(flywheelVoltageOffset);
+        table.getEntry("backboardReady").setBoolean(isBackboardReady());
+        table.getEntry("backboardPositionError").setDouble(shooter.getBackboardPositionError());
+        table.getEntry("backboardReadyTolerance").setDouble(Constants.Superstructure.backboardReadyPositionTolerance);
+        table.getEntry("flywheelVelocityOffset").setDouble(flywheelVelocityOffset);
+        table.getEntry("flywheelTuningControlActive").setBoolean(shooter.isFlywheelTuningControlActive());
+        table.getEntry("shooterConveyorTuningControlActive")
+            .setBoolean(shooter.isShooterConveyorTuningControlActive());
+        table.getEntry("flywheelVoltageOffset").setDouble(flywheelVelocityOffset * Constants.Shooter.flywheelNominalKv);
        table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
    }
 }
@@ -1,6 +1,4 @@
 package frc.robot.tuning;
-import java.util.function.Function;
-
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -25,6 +23,7 @@ public class ConfigTalonFXMotorTuner {
  private final BooleanSubscriber enableRuningSub;
  private final DoubleSubscriber runningVolts;
  private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
+  private boolean wasRunning = false;
  
  private final VoltageOut voltageRequest = new VoltageOut(0);
  public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
@@ -83,9 +82,11 @@ public class ConfigTalonFXMotorTuner {
    if (enableRuningSub.get()) {;
      double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0);
      motor.setControl(voltageRequest.withOutput(volts));
+      wasRunning = true;
    // System.out.println("running motor with volts: "+volts+" on motor: "+motorName);
-    }else{
+    }else if (wasRunning){
      motor.setControl(voltageRequest.withOutput(0));
+      wasRunning = false;
    }
  }