7.21

elastic-layout-debug.json

@@ -2196,6 +2196,492 @@
         ]
       },
       "title": "Flywheel Debug"
+    },
+    {
+      "name": "Conveyor Debug",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Shooter Velocity Test",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Toggle Switch",
+            "properties": {
+              "topic": "/Tuning/shooterConveyor/shooterConveyorVelocityEnable",
+              "period": 0.06,
+              "data_type": "boolean"
+            }
+          },
+          {
+            "title": "Shooter Test Active",
+            "x": 128.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/Tuning/shooterConveyor/shooterConveyorVelocityEnableActive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Shooter Holdoff",
+            "x": 320.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/Superstructure/shooterConveyorTuningControlActive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Main Voltage Test",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Toggle Switch",
+            "properties": {
+              "topic": "/Tuning/mainConveyor/mainConveyorVoltageEnable",
+              "period": 0.06,
+              "data_type": "boolean"
+            }
+          },
+          {
+            "title": "Main Test Active",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/Tuning/mainConveyor/mainConveyorVoltageEnableActive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Main Holdoff",
+            "x": 1088.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/Superstructure/mainConveyorTuningControlActive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Shooter Target Velocity",
+            "x": 0.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/shooterConveyor/shooterConveyorTargetVelocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Shooter Commanded",
+            "x": 256.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/shooterConveyor/shooterConveyorCommandedTargetVelocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Clamped",
+            "x": 512.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/shooterConveyor/shooterConveyorClampedTargetVelocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Main Target Volts",
+            "x": 768.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/mainConveyor/mainConveyorTargetVolts",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Main Commanded",
+            "x": 1024.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/mainConveyor/mainConveyorCommandedTargetVolts",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Main Clamped",
+            "x": 1280.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/mainConveyor/mainConveyorClampedTargetVolts",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Target Now",
+            "x": 0.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Shooter/conveyorTargetVelocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Velocity",
+            "x": 256.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Shooter/conveyorVelocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Error",
+            "x": 512.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Shooter/conveyorVelocityError",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Main Target Now",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Conveyor/targetOutputVolts",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Main Velocity",
+            "x": 1024.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Conveyor/velocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Main Output Volts",
+            "x": 1280.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Conveyor/motorOutputVolts",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Output Volts",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Shooter/shooterConveyorOutputVolts",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Left Current",
+            "x": 256.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Shooter/conveyorLeftStatorCurrent",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Right Current",
+            "x": 512.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Shooter/conveyorRightStatorCurrent",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Main Current",
+            "x": 768.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Conveyor/statorCurrent",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter Max Velocity",
+            "x": 1024.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/shooterConveyor/shooterConveyorMaxVelocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter NT Active",
+            "x": 1280.0,
+            "y": 384.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/Shooter/conveyorVelocityTuningActive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Shooter Velocity",
+            "x": 0.0,
+            "y": 512.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Shooter/conveyorVelocity",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4278238420,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "Shooter Target",
+            "x": 384.0,
+            "y": 512.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Shooter/conveyorTargetVelocity",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4283443184,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "Shooter Error",
+            "x": 768.0,
+            "y": 512.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Shooter/conveyorVelocityError",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4294198070,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "Shooter Output Volts",
+            "x": 1152.0,
+            "y": 512.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Shooter/shooterConveyorOutputVolts",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4289374890,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "Main Velocity",
+            "x": 0.0,
+            "y": 768.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Conveyor/velocity",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4278238420,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "Main Target Volts",
+            "x": 384.0,
+            "y": 768.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Conveyor/targetOutputVolts",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4283443184,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "Main Output Volts",
+            "x": 768.0,
+            "y": 768.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Conveyor/motorOutputVolts",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4289374890,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "Main Current",
+            "x": 1152.0,
+            "y": 768.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Conveyor/statorCurrent",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 8.0,
+              "color": 4294198070,
+              "line_width": 2.0
+            }
+          }
+        ]
+      },
+      "title": "Conveyor Debug"
     }
   ]
 }

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

@@ -30,6 +30,7 @@ public final class Constants {
 
         public static final double shooterConveyorFeedVelocity = 40;
         public static final double shooterConveyorReverseVelocity = -10;
+        public static final double shooterConveyorMaxVelocity = 120.0;
 
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
             .withKP(0.21)
@@ -128,7 +129,7 @@ public final class Constants {
         public static final double intakeRollerOuttakeVoltage = 3.0;
 
         public static final double intakeRotaterRaiseVoltage = 0.8;
-        public static final double intakeRotaterLowerVoltage = -0.1;
+        public static final double intakeRotaterLowerVoltage = -1.5;
 
         public static final Slot0Configs intakeRollerSlot0Configs = new Slot0Configs()
             .withKP(0.00).withKI(0.00).withKD(0.00)
@@ -146,6 +147,7 @@ public final class Constants {
     public static class Conveyor {
         public static final int mainConveyorID = 42;
         public static final double feedVoltage = 2.5;
+        public static final double intakeFeedVoltage = 1.5;
         public static final double reverseVoltage = -2.0;
 
         public static final Slot0Configs mainConveyorSlot0Configs = new Slot0Configs()
@@ -159,6 +161,7 @@ public final class Constants {
         public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity;
         public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity;
         public static final double shootTimeoutSeconds = 4.0;
+        public static final double prepShootFallbackDelaySeconds = 0.5;
         public static final double aimToleranceDegrees = 1.5;
         public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
         public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage;
@@ -168,7 +171,5 @@ public final class Constants {
         // Shoot backboard target position in external encoder counts.
         public static final double backboardShootPosition = 175.0;
         public static final double backboardReadyPositionTolerance = 100.0;
-        // Whether the shooting gate requires the drivetrain to be nearly still.
-        public static final boolean stationaryGateEnabled = true;
     }
 }

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

@@ -138,6 +138,9 @@ public class RobotContainer {
         .onTrue(operatorControls.requestShootHubCommand())
         .onFalse(operatorControls.requestIdleCommand());
     controller.rightBumper().and(teleopOnly)
+        .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
+        .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
+    controller.b().and(teleopOnly)
         .onTrue(operatorControls.requestEjectCommand())
         .onFalse(operatorControls.requestIdleCommand());
     controller.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0)));
@@ -162,6 +165,8 @@ public class RobotContainer {
     configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
     configBackboardTuner.periodic();
     shooterSubsystem.debugFlywheelVelocityPeriodic(allowMotorOutputTuning);
+    shooterSubsystem.debugShooterConveyorVelocityPeriodic(allowMotorOutputTuning);
+    conveyorSubsystem.debugMainConveyorVoltagePeriodic(allowMotorOutputTuning);
     shooterSubsystem.debugBackboardTargetPeriodic(allowMotorOutputTuning);
     shooterSubsystem.setRightFlywheelFollowLeft();
     shooterSubsystem.setRightConveyorFollowLeft();

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

@@ -37,14 +37,14 @@ public class DriveControls {
     private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
 
     // 默认速度比例，普通驾驶使用。
-    private double normalDriveScale = 1.0;
+    private double normalDriveScale = 0.6;
-    private double normalTurnScale  = 1.0;
+    private double normalTurnScale  = 0.8;
 
     // 加速模式比例，按住 RB 时使用。
     private double boostDriveScale = 1.0;
     private double boostTurnScale  = 1.0;
     // 自动瞄准时降低平移速度。
-    private double slowDriveScale =0.1;
+    private double slowDriveScale = 0.3;
 
     private boolean boostEnabled = false;
 

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

@@ -1,11 +1,14 @@
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.math.MathUtil;
 import frc.robot.Constants;
@@ -18,15 +21,27 @@ public class ConveyorSubsystem extends SubsystemBase {
     private final VoltageOut voltageRequest = new VoltageOut(0);
     private final NeutralOut neutralRequest = new NeutralOut();
     private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
+    private final NetworkTable tuningTable = NetworkTableInstance.getDefault().getTable("Tuning/mainConveyor");
     private double targetOutputVolts = 0.0;
+    private double commandedDebugVolts = Constants.Conveyor.feedVoltage;
+    private double clampedDebugVolts = Constants.Conveyor.feedVoltage;
+    private boolean voltageTuningActive = false;
 
     public ConveyorSubsystem() {
         mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
         mainConveyor.getConfigurator().apply(Constants.Conveyor.mainConveyorSlot0Configs);
+        var mainConveyorMotorOutputConfigs = new MotorOutputConfigs();
+        mainConveyorMotorOutputConfigs.NeutralMode = NeutralModeValue.Coast;
+        mainConveyor.getConfigurator().apply(mainConveyorMotorOutputConfigs);
+        tuningTable.getEntry("mainConveyorVoltageEnable").setDefaultBoolean(false);
+        tuningTable.getEntry("mainConveyorTargetVolts").setDefaultDouble(Constants.Conveyor.feedVoltage);
     }
     public void feedToShooter() {
         setVoltage(Constants.Conveyor.feedVoltage);
     }
+    public void feedForIntake() {
+        setVoltage(Constants.Conveyor.intakeFeedVoltage);
+    }
     public void reverse() {
         setVoltage(Constants.Conveyor.reverseVoltage);
     }
@@ -39,9 +54,37 @@ public class ConveyorSubsystem extends SubsystemBase {
         mainConveyor.setControl(voltageRequest.withOutput(targetOutputVolts));
     }
 
+    public boolean isMainConveyorTuningControlActive() {
+        return DriverStation.isTestEnabled()
+            && tuningTable.getEntry("mainConveyorVoltageEnable").getBoolean(false);
+    }
+
+    public void debugMainConveyorVoltagePeriodic(boolean allowMotorOutput) {
+        voltageTuningActive =
+            allowMotorOutput
+                && DriverStation.isTestEnabled()
+                && tuningTable.getEntry("mainConveyorVoltageEnable").getBoolean(false);
+        commandedDebugVolts = tuningTable.getEntry("mainConveyorTargetVolts")
+            .getDouble(Constants.Conveyor.feedVoltage);
+        clampedDebugVolts = MathUtil.clamp(commandedDebugVolts, -12.0, 12.0);
+
+        tuningTable.getEntry("mainConveyorVoltageEnableActive").setBoolean(voltageTuningActive);
+        tuningTable.getEntry("mainConveyorCommandedTargetVolts").setDouble(commandedDebugVolts);
+        tuningTable.getEntry("mainConveyorClampedTargetVolts").setDouble(clampedDebugVolts);
+
+        if (voltageTuningActive) {
+            setVoltage(clampedDebugVolts);
+        }
+    }
+
     @Override
     public void periodic() {
         table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
         table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
+        table.getEntry("motorOutputVolts").setDouble(mainConveyor.getMotorVoltage().getValueAsDouble());
+        table.getEntry("statorCurrent").setDouble(mainConveyor.getStatorCurrent().getValueAsDouble());
+        table.getEntry("voltageTuningActive").setBoolean(voltageTuningActive);
+        table.getEntry("commandedDebugVolts").setDouble(commandedDebugVolts);
+        table.getEntry("clampedDebugVolts").setDouble(clampedDebugVolts);
     }
 }

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

@@ -69,9 +69,9 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
     }
 
     public void shootAssist() {
-        double cycleTime = Timer.getFPGATimestamp() % 1.7;
+        double cycleTime = Timer.getFPGATimestamp() % 1.6;
 
-        if (cycleTime < 0.7) {
+        if (cycleTime < 0.6) {
             setVoltage(1);
         } else if (cycleTime < 0.9) {
             setVoltage(0.0);

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

@@ -54,12 +54,15 @@ public class ShooterSubsystem extends SubsystemBase {
     private final NetworkTable shooterNetworkTable;
     private final NetworkTable flywheelTuningTable;
     private final NetworkTable backboardTuningTable;
+    private final NetworkTable shooterConveyorTuningTable;
 
     private double flywheelOutputVolts = 0.0;
     private double flywheelTargetVelocity = 0.0;
     private double flywheelVelocityTolerance = Constants.Shooter.flywheelReadyVelocityTolerance;
     private boolean flywheelVelocityTuningActive = false;
     private double shooterConveyorTargetVelocity = 0.0;
+    private double shooterConveyorCommandedTargetVelocity = Constants.Shooter.shooterConveyorFeedVelocity;
+    private double shooterConveyorClampedTargetVelocity = Constants.Shooter.shooterConveyorFeedVelocity;
     private boolean shooterConveyorVelocityTuningActive = false;
     private double shooterConveyorOutputVolts = 0.0;
     private double backboardOutputVolts = 0.0;
@@ -123,6 +126,10 @@ public class ShooterSubsystem extends SubsystemBase {
         backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
         backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
         backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
+        shooterConveyorTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/shooterConveyor");
+        shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnable").setDefaultBoolean(false);
+        shooterConveyorTuningTable.getEntry("shooterConveyorTargetVelocity")
+            .setDefaultDouble(Constants.Shooter.shooterConveyorFeedVelocity);
     }
 
     /* ====================== */
@@ -354,7 +361,8 @@ public class ShooterSubsystem extends SubsystemBase {
     }
 
     public boolean isShooterConveyorTuningControlActive() {
-        return false;
+        return DriverStation.isTestEnabled()
+            && shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnable").getBoolean(false);
     }
 
     public void resetbackboardencoder() {
@@ -429,6 +437,34 @@ public class ShooterSubsystem extends SubsystemBase {
         }
     }
 
+    public void debugShooterConveyorVelocityPeriodic(boolean allowMotorOutput) {
+        shooterConveyorVelocityTuningActive =
+            allowMotorOutput
+                && DriverStation.isTestEnabled()
+                && shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnable").getBoolean(false);
+        shooterConveyorCommandedTargetVelocity = shooterConveyorTuningTable
+            .getEntry("shooterConveyorTargetVelocity")
+            .getDouble(Constants.Shooter.shooterConveyorFeedVelocity);
+        shooterConveyorClampedTargetVelocity = MathUtil.clamp(
+            shooterConveyorCommandedTargetVelocity,
+            -Constants.Shooter.shooterConveyorMaxVelocity,
+            Constants.Shooter.shooterConveyorMaxVelocity
+        );
+
+        shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnableActive")
+            .setBoolean(shooterConveyorVelocityTuningActive);
+        shooterConveyorTuningTable.getEntry("shooterConveyorCommandedTargetVelocity")
+            .setDouble(shooterConveyorCommandedTargetVelocity);
+        shooterConveyorTuningTable.getEntry("shooterConveyorClampedTargetVelocity")
+            .setDouble(shooterConveyorClampedTargetVelocity);
+        shooterConveyorTuningTable.getEntry("shooterConveyorMaxVelocity")
+            .setDouble(Constants.Shooter.shooterConveyorMaxVelocity);
+
+        if (shooterConveyorVelocityTuningActive) {
+            runShooterConveyorVelocity(shooterConveyorClampedTargetVelocity);
+        }
+    }
+
     @Override
     public void periodic() {
         double backboardPosition = getBackboardPosition();
@@ -456,6 +492,10 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("conveyorVelocity").setDouble(getShooterConveyorVelocity());
         shooterNetworkTable.getEntry("conveyorVelocityError").setDouble(getShooterConveyorVelocityError());
         shooterNetworkTable.getEntry("conveyorVelocityTuningActive").setBoolean(shooterConveyorVelocityTuningActive);
+        shooterNetworkTable.getEntry("conveyorCommandedTargetVelocity").setDouble(shooterConveyorCommandedTargetVelocity);
+        shooterNetworkTable.getEntry("conveyorClampedTargetVelocity").setDouble(shooterConveyorClampedTargetVelocity);
+        shooterNetworkTable.getEntry("conveyorLeftStatorCurrent").setDouble(leftConveyor.getStatorCurrent().getValueAsDouble());
+        shooterNetworkTable.getEntry("conveyorRightStatorCurrent").setDouble(rightConveyor.getStatorCurrent().getValueAsDouble());
         shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
         shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
         shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);

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

@@ -182,10 +182,8 @@ public class Superstructure extends SubsystemBase {
     /* ====================== */
 
     public boolean canShoot() {
-        boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary();
         return shooter.atVelocity()
             && drive.isAimed()
-            && stationaryGate
             && isBackboardReady();
     }
 
@@ -257,7 +255,7 @@ public class Superstructure extends SubsystemBase {
             setSystemState(SystemState.INITIALIZING);
         }
 
-        conveyor.stop();
+        stopMainConveyorUnlessTuning();
         intakeRoller.stop();
         stopFlywheelUnlessTuning();
         stopShooterConveyorUnlessTuning();
@@ -266,7 +264,7 @@ public class Superstructure extends SubsystemBase {
 
     private void handleIdle() {
         // 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。
-        conveyor.stop();
+        stopMainConveyorUnlessTuning();
         intakeRoller.stop();
         if (!runPostShootIdleAssist()) {
             intakeRotater.stop();
@@ -298,13 +296,13 @@ public class Superstructure extends SubsystemBase {
         holdBackboardStowedUnlessTuning();
         // Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。
         intakeRoller.intake();
-        conveyor.feedToShooter();
+        intakeFeedMainConveyorUnlessTuning();
         setSystemState(SystemState.INTAKING);
     }
 
     private void handleAimHub() {
         intakeRoller.stop();
-        conveyor.stop();
+        stopMainConveyorUnlessTuning();
         stopFlywheelUnlessTuning();
         stopShooterConveyorUnlessTuning();
         holdBackboardStowedUnlessTuning();
@@ -323,8 +321,8 @@ public class Superstructure extends SubsystemBase {
     }
 
     private void handlePassBall() {
-        double targetVelocity = Constants.Superstructure.passBallFlywheelVelocity + flywheelVelocityOffset;
+        double targetVelocity = 30;
-        double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
+        double backboardPosition = 2000;
         handlePreparedFeed(targetVelocity, backboardPosition);
     }
 
@@ -343,27 +341,32 @@ public class Superstructure extends SubsystemBase {
             // 避免球接触飞轮导致的掉速中断喂球。
             intakeRotater.shootAssist();
             intakeRoller.intakeforshoot();
-            conveyor.feedToShooter();
+            feedMainConveyorUnlessTuning();
             runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity);
             return;
         }
 
         // 未开始喂球前，球路保持静止。
-        conveyor.stop();
+        stopMainConveyorUnlessTuning();
         stopShooterConveyorUnlessTuning();
         intakeRoller.stop();
 
-        // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
+        // Keep normal gating, but force the shot if prep takes too long.
-        if (canShoot()) {
+        if (canShoot() || prepShootFallbackElapsed()) {
             setSystemState(SystemState.SHOOTING);
         }
     }
 
+    private boolean prepShootFallbackElapsed() {
+        return systemState == SystemState.PREP_SHOOT
+            && timeInState() >= Constants.Superstructure.prepShootFallbackDelaySeconds;
+    }
+
     private void handleEject() {
         // Eject 反转整条球路，飞轮保持停止。
         setSystemState(SystemState.EJECTING);
         intakeRoller.outtake();
-        conveyor.reverse();
+        reverseMainConveyorUnlessTuning();
         runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterReverseVelocity);
         stopFlywheelUnlessTuning();
         holdBackboardStowedUnlessTuning();
@@ -393,13 +396,37 @@ public class Superstructure extends SubsystemBase {
         }
     }
 
+    private void feedMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.feedToShooter();
+        }
+    }
+
+    private void intakeFeedMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.feedForIntake();
+        }
+    }
+
+    private void reverseMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.reverse();
+        }
+    }
+
+    private void stopMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.stop();
+        }
+    }
+
     private void handleManual() {
         // 手动模式：状态机不再驱动机构，交给现场手动/调参工具；
         // 只保持飞轮与喂球停止，避免意外射出。
         setSystemState(SystemState.MANUAL_OVERRIDE);
         stopFlywheelUnlessTuning();
         stopShooterConveyorUnlessTuning();
-        conveyor.stop();
+        stopMainConveyorUnlessTuning();
         intakeRoller.stop();
     }
 
@@ -441,6 +468,8 @@ public class Superstructure extends SubsystemBase {
         table.getEntry("flywheelTuningControlActive").setBoolean(shooter.isFlywheelTuningControlActive());
         table.getEntry("shooterConveyorTuningControlActive")
             .setBoolean(shooter.isShooterConveyorTuningControlActive());
+        table.getEntry("mainConveyorTuningControlActive")
+            .setBoolean(conveyor.isMainConveyorTuningControlActive());
         table.getEntry("flywheelVoltageOffset").setDouble(flywheelVelocityOffset * Constants.Shooter.flywheelNominalKv);
         table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
     }