diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1a0d313..507b813 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -132,18 +132,16 @@ public final class Constants { public static final double intakeRollerVoltage = 6.0; public static final double outtakeRollerVoltage = -5.0; - public static final double holdRollerVoltage = 1.0; - public static final double intakeRotaterStowPosition = 0.0; - public static final double intakeRotaterGroundPosition = -7.6; - public static final double intakeRotaterHandoffPosition = -2.5; - public static final double intakeRotaterSafePosition = 0.0; - public static final double intakeRotaterManualVoltage = 2.0; + public static final double intakeRotaterLoweredPosition = -7.6; + public static final double intakeRotaterShootAssistUpPosition = -2.5; + public static final double intakeRotaterManualRaiseVoltage = 2.0; + public static final double intakeRotaterManualLowerVoltage = -2.0; + public static final double intakeRotaterShootAssistPeriodSeconds = 0.4; + public static final double intakeRotaterShootAssistUpDutyCycle = 0.5; public static final double intakeRotaterUpLimit = 0.0; public static final double intakeRotaterDownLimit = -7.6; - public static final double intakeRotaterVelocity = 5; - public static final double IntakeRotaterGravityFF = 0.0; public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs() .withKP(0.00).withKI(0.00).withKD(0.00) @@ -154,16 +152,8 @@ public final class Constants { .withKV(0.00).withKA(0.00).withKS(0.00); public static final double IntakeGetterSpeedforAuto = 0.5; - // atGoal 判定容差。为 0 会导致 atGoal 永远为 false、射击门控永远不通过； // 这里给出可用的占位值，真车调好位置闭环后再收紧。 public static final double positionToleranceRotations = 0.3; - - // 上电时按机构处于收拢（STOW）姿态直接归零。 - // 比赛规则要求开赛前机器人处于合法初始姿态，因此这是安全假设； - // 若维修后姿态不确定，置 false 并用操作员 A 键手动归零。 - // 没有这个假设，自动赛阶段无人能按归零键，收球和射击会被 - // 未归零保护永久拦下。 - public static final boolean assumeZeroedOnBoot = true; } public static class Conveyor { @@ -181,17 +171,8 @@ public final class Constants { public static final double aimToleranceDegrees = 1.5; public static final double stationarySpeedToleranceMetersPerSecond = 0.5; - // 松开 intake 键后，INDEXING 状态继续收纳球的时长。 - public static final double indexingSeconds = 0.5; // 射击时背板的目标位置（外接编码器计数）。 public static final double backboardShootPosition = 175.0; - // 没有球检测传感器，持球状态由状态机跟踪推断。 - // 若跟踪不可靠导致射击被卡，可临时置 false 旁路 hasNote 门控。 - public static final boolean hasNoteGateEnabled = true; - // 比赛开局机器人带预装球，开机默认持球为 true； - // 否则自动赛的第一发射击会被 hasNote 门控拦死。 - // 空车调试时可置 false，或用操作员 X 键运行时翻转持球状态。 - public static final boolean assumePreloadedAtBoot = true; // 射击门控是否要求底盘接近静止。 public static final boolean stationaryGateEnabled = true; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a3d693e..35ca4d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,6 +98,7 @@ public class RobotContainer { configueIntaker(); Autocommand.preNameCommands( superstructure, + intakeRotaterSubsystem, driveControls, operatorControls ); @@ -174,7 +175,7 @@ public class RobotContainer { private void configueShooter() { // 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure - // 根据飞轮速度、底盘瞄准和进球旋转机构位置统一判断。 + // 根据飞轮速度和底盘瞄准统一判断。 // 这些命令 require Superstructure，与自动程序共用需求； // 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。 Trigger teleopOnly = RobotModeTriggers.teleop(); @@ -187,8 +188,6 @@ public class RobotContainer { controllerupper.b().and(teleopOnly) .onTrue(operatorControls.requestEjectCommand()) .onFalse(operatorControls.requestIdleCommand()); - // 持球状态是无传感器推断值，X 键允许操作员运行时纠正。 - controllerupper.x().onTrue(operatorControls.toggleNotePresentCommand()); controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(-1))); controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(1))); controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100))); @@ -218,11 +217,22 @@ public class RobotContainer { } private void configueIntaker() { - // 进球相关按钮也只请求 Superstructure 状态。 - // A 键是校准动作，只应在进球机构处于已知零点姿态时按下。 - controllerupper.leftTrigger().and(RobotModeTriggers.teleop()) + // 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。 + Trigger teleopOnly = RobotModeTriggers.teleop(); + controllerupper.leftTrigger().and(teleopOnly) .onTrue(operatorControls.requestIntakeCommand()) .onFalse(operatorControls.requestIdleCommand()); - controllerupper.a().onTrue(operatorControls.resetIntakeRotaterEncoderCommand(intakeRotaterSubsystem)); + + Trigger manualRaiseIntaker = controllerupper.x().and(teleopOnly); + manualRaiseIntaker + .onTrue(operatorControls.requestManualCommand()) + .whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem)) + .onFalse(operatorControls.requestIdleCommand()); + + Trigger manualLowerIntaker = controllerupper.a().and(teleopOnly); + manualLowerIntaker + .onTrue(operatorControls.requestManualCommand()) + .whileTrue(operatorControls.manualLowerIntakeRotaterCommand(intakeRotaterSubsystem)) + .onFalse(operatorControls.requestIdleCommand()); } } diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index c79d392..f257500 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/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.subsystems.IntakeRotaterSubsystem; import frc.robot.subsystems.Superstructure; /** @@ -16,6 +17,7 @@ import frc.robot.subsystems.Superstructure; public class Autocommand extends SequentialCommandGroup { public static void preNameCommands( Superstructure superstructure, + IntakeRotaterSubsystem intakeRotater, DriveControls driveControls, OperatorControls operatorControls ) { @@ -24,7 +26,7 @@ public class Autocommand extends SequentialCommandGroup { NamedCommands.registerCommand("stop_intake_and_conveyor", idle(superstructure)); NamedCommands.registerCommand("start_intake_timelimit", timedIntake(superstructure)); NamedCommands.registerCommand("stop_intake", idle(superstructure)); - NamedCommands.registerCommand("intaker_down", startIntake(superstructure)); + NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater)); NamedCommands.registerCommand("intaker_up", idle(superstructure)); NamedCommands.registerCommand("shoot", shoot(superstructure)); NamedCommands.registerCommand("Shake", shake(driveControls)); @@ -49,6 +51,10 @@ public class Autocommand extends SequentialCommandGroup { return superstructure.requestIdleCommand(); } + public static Command lowerIntaker(IntakeRotaterSubsystem intakeRotater) { + return intakeRotater.lowerForMatchCommand(); + } + // 自动射击和手动射击使用同一套 gated shooting 流程。 public static Command shoot(Superstructure superstructure) { return Commands.sequence( diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index 15afbfa..6bb1068 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -45,10 +45,6 @@ public class OperatorControls { return superstructure.requestManualCommand(); } - public Command toggleNotePresentCommand() { - return superstructure.toggleNotePresentCommand(); - } - public void adjustFlywheelSpeedOffset(double offset) { superstructure.adjustFlywheelSpeedOffset(offset); } @@ -65,8 +61,12 @@ public class OperatorControls { return Commands.runOnce(superstructure::resetBackboardEncoder, shooterSubsystem); } - public Command resetIntakeRotaterEncoderCommand(IntakeRotaterSubsystem intakeRotater) { - return Commands.runOnce(intakeRotater::zeroPosition, intakeRotater); + public Command manualRaiseIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) { + return Commands.runEnd(intakeRotater::manualRaise, intakeRotater::stop, intakeRotater); + } + + public Command manualLowerIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) { + return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater); } // PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球， diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java index 43268b1..efef84f 100644 --- a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java @@ -3,7 +3,6 @@ package frc.robot.subsystems; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -11,18 +10,12 @@ import frc.robot.Constants; /** * 主 conveyor 的开环控制封装。 - * - * 本赛季没有球检测传感器，持球状态由 Superstructure 按状态流程推断后 - * 写入这里（INDEXING 完成置位，射击/EJECT 后清空）； - * 所有动作顺序判断都放在 Superstructure。 */ public class ConveyorSubsystem extends SubsystemBase { public final TalonFX mainConveyor; private final VoltageOut voltageRequest = new VoltageOut(0); private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor"); - private double voltage = 0.0; - public ConveyorSubsystem() { mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID); } @@ -42,6 +35,5 @@ public class ConveyorSubsystem extends SubsystemBase { @Override public void periodic() { table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble()); - table.getEntry("hasNote").setBoolean(notePresent); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java index 3a5376c..2fa9511 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java @@ -9,40 +9,29 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; /** - * 进球旋转臂的位置闭环和开环控制。 + * Intaker rotation control for the 2026 fuel path. * - * 机构使用 TalonFX 集成编码器。归零前只允许 SAFE 或受限手动移动， - * 避免相对编码器未校准时发生大幅度意外旋转。 + * The normal match posture is lowered. During shooting, the arm alternates + * between lowered and an upward assist position to help fuel enter the chamber. */ public class IntakeRotaterSubsystem extends SubsystemBase { - public enum IntakePosition { - STOW(Constants.Intaker.intakeRotaterStowPosition), - GROUND_INTAKE(Constants.Intaker.intakeRotaterGroundPosition), - HANDOFF(Constants.Intaker.intakeRotaterHandoffPosition), - SAFE(Constants.Intaker.intakeRotaterSafePosition); - - public final double rotations; - - IntakePosition(double rotations) { - this.rotations = rotations; - } - } - - public final TalonFX leftIntakeRotater; - public final TalonFX rightIntakeRotater; + private final TalonFX leftIntakeRotater; + private final TalonFX rightIntakeRotater; private final PositionVoltage positionRequest = new PositionVoltage(0); private final VoltageOut voltageRequest = new VoltageOut(0); private final Follower rightFollower; private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater"); - private IntakePosition goal = IntakePosition.SAFE; - private boolean zeroed = false; - private double manualVoltage = 0.0; + private double targetPosition = Constants.Intaker.intakeRotaterLoweredPosition; + private double outputVoltage = 0.0; public IntakeRotaterSubsystem() { leftIntakeRotater = new TalonFX(Constants.Intaker.leftIntakeRotaterID); @@ -52,100 +41,61 @@ public class IntakeRotaterSubsystem extends SubsystemBase { rightFollower = new Follower(leftIntakeRotater.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); followLeft(); - if (Constants.Intaker.assumeZeroedOnBoot) { - // 开机默认机构处于收拢姿态，直接把当前位置记为零点， - // 让自动赛阶段的收球/射击门控可以工作；只标定编码器，不命令运动。 - leftIntakeRotater.setPosition(0.0); - rightIntakeRotater.setPosition(0.0); - zeroed = true; - goal = IntakePosition.STOW; - } + leftIntakeRotater.setPosition(0.0); + rightIntakeRotater.setPosition(0.0); } - public void setGoal(IntakePosition position) { - // 相对编码器没有对准机械零点前，禁止执行大幅度位置闭环移动。 - goal = position; - if (!zeroed && position != IntakePosition.SAFE) { - stop(); - return; - } - - double target = MathUtil.clamp( - position.rotations, - Constants.Intaker.intakeRotaterDownLimit, - Constants.Intaker.intakeRotaterUpLimit - ); - leftIntakeRotater.setControl(positionRequest.withPosition(target)); - manualVoltage = 0.0; - followLeft(); + public void lowerForMatch() { + setPosition(Constants.Intaker.intakeRotaterLoweredPosition); } - public void stow() { - setGoal(IntakePosition.STOW); + public Command lowerForMatchCommand() { + return Commands.runOnce(this::lowerForMatch, this); } - public void deployToGround() { - setGoal(IntakePosition.GROUND_INTAKE); + public void shootAssist() { + double period = Constants.Intaker.intakeRotaterShootAssistPeriodSeconds; + double phase = (Timer.getFPGATimestamp() % period) / period; + double target = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle + ? Constants.Intaker.intakeRotaterShootAssistUpPosition + : Constants.Intaker.intakeRotaterLoweredPosition; + setPosition(target); } - public void moveToHandoff() { - setGoal(IntakePosition.HANDOFF); + public void manualRaise() { + setVoltage(Constants.Intaker.intakeRotaterManualRaiseVoltage); + } + + public void manualLower() { + setVoltage(Constants.Intaker.intakeRotaterManualLowerVoltage); + } + + public void stop() { + setVoltage(0.0); } public boolean atGoal() { - // 未归零时编码器读数没有意义，一律视为未到位， - // 让上层射击门控和状态流转不会被假读数放行。 - if (!zeroed) { - return false; - } - return Math.abs(getPosition() - goal.rotations) <= Constants.Intaker.positionToleranceRotations; - } - - public boolean isBlockedByNotZeroed() { - // 归零前收到了 SAFE 以外的位置请求：机构被安全逻辑拦下，需要先归零。 - return !zeroed && goal != IntakePosition.SAFE; - } - - public void zeroPosition() { - // 只有 intake 物理上处于已知零点姿态时才调用此方法。 - leftIntakeRotater.setPosition(0.0); - rightIntakeRotater.setPosition(0.0); - zeroed = true; - goal = IntakePosition.STOW; - stow(); - } - - public boolean isZeroed() { - return zeroed; - } - - public void clearZeroed() { - zeroed = false; - goal = IntakePosition.SAFE; - stop(); + return Math.abs(getPosition() - targetPosition) <= Constants.Intaker.positionToleranceRotations; } public double getPosition() { return leftIntakeRotater.getPosition().getValueAsDouble(); } - public IntakePosition getGoal() { - return goal; - } - - public void setOpenLoopVoltage(double voltage) { - // 归零前允许手动小幅移动，但输出会被限制到较温和的范围。 - if (!zeroed && Math.abs(voltage) > Constants.Intaker.intakeRotaterManualVoltage) { - voltage = Math.copySign(Constants.Intaker.intakeRotaterManualVoltage, voltage); - } - manualVoltage = MathUtil.clamp(voltage, -12.0, 12.0); - leftIntakeRotater.setControl(voltageRequest.withOutput(manualVoltage)); + private void setPosition(double rotations) { + targetPosition = MathUtil.clamp( + rotations, + Constants.Intaker.intakeRotaterDownLimit, + Constants.Intaker.intakeRotaterUpLimit + ); + outputVoltage = 0.0; + leftIntakeRotater.setControl(positionRequest.withPosition(targetPosition)); followLeft(); } - public void stop() { - manualVoltage = 0.0; - leftIntakeRotater.setControl(voltageRequest.withOutput(0.0)); + private void setVoltage(double volts) { + outputVoltage = MathUtil.clamp(volts, -12.0, 12.0); + leftIntakeRotater.setControl(voltageRequest.withOutput(outputVoltage)); followLeft(); } @@ -155,11 +105,9 @@ public class IntakeRotaterSubsystem extends SubsystemBase { @Override public void periodic() { - table.getEntry("goal").setString(goal.name()); - table.getEntry("zeroed").setBoolean(zeroed); - table.getEntry("blockedByNotZeroed").setBoolean(isBlockedByNotZeroed()); table.getEntry("position").setDouble(getPosition()); + table.getEntry("targetPosition").setDouble(targetPosition); table.getEntry("atGoal").setBoolean(atGoal()); - table.getEntry("manualVoltage").setDouble(manualVoltage); + table.getEntry("outputVoltage").setDouble(outputVoltage); } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 3ac4f9e..e3b1072 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -18,6 +18,7 @@ public class LEDSubsystem extends SubsystemBase { private final AddressableLED led; private final AddressableLEDBuffer buffer; private final Timer timer = new Timer(); + private final boolean enabled; // 左右灯带分段（左 0~71，右 72~143） private final int leftStart = 0; @@ -50,11 +51,15 @@ public class LEDSubsystem extends SubsystemBase { led = new AddressableLED(pwmPort); buffer = new AddressableLEDBuffer(leftLen + rightLen); + enabled = Constants.LEDUsing; - led.setLength(buffer.getLength()); - led.start(); - - timer.start(); + if (enabled) { + led.setLength(buffer.getLength()); + led.start(); + timer.start(); + } else { + led.close(); + } } /* ====================== */ @@ -93,7 +98,9 @@ public class LEDSubsystem extends SubsystemBase { @Override public void periodic() { - if(!Constants.LEDUsing) {led.close(); return;} + if (!enabled) { + return; + } // 左段 switch (leftMode) { case OFF -> setRange(leftStart, leftLen, 0, 0, 0); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 85dcc32..3d3ba0c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -49,8 +49,6 @@ public class ShooterSubsystem extends SubsystemBase { private final NetworkTable shooterNetworkTable; private double targetFlywheelRps = 0.0; - private double targetLeftFlywheelRps = 0.0; - private double targetRightFlywheelRps = 0.0; private double shooterConveyorPercent = 0.0; public ShooterSubsystem() { @@ -108,12 +106,10 @@ public class ShooterSubsystem extends SubsystemBase { rightFlywheel.getConfigurator().apply(configs); } - public void setFlywheelVelocity(double leftRps, double rightRps) { - // 对外接口使用 RPS，便于 Superstructure 阅读；实际下发仍是电压开环。 - targetLeftFlywheelRps = leftRps; - targetRightFlywheelRps = rightRps; - targetFlywheelRps = (Math.abs(leftRps) + Math.abs(rightRps)) / 2.0; - + public void setFlywheelVelocity(double targetRps) { + // 右飞轮是 follower，因此对外只暴露一个飞轮目标速度。 + // 实际控制仍是左飞轮电压开环，右飞轮反向跟随左飞轮。 + targetFlywheelRps = Math.abs(targetRps); double requestedVoltage = targetFlywheelRps * Constants.Shooter.flywheelVoltsPerRps; requestedVoltage = MathUtil.clamp(requestedVoltage, -12.0, 12.0); leftFlywheel.setControl(flywheelVoltageRequest.withOutput(requestedVoltage)); @@ -123,12 +119,10 @@ public class ShooterSubsystem extends SubsystemBase { public void setFlywheelVelocityByDistance(double distance) { // 查 Constants 里的射表，两端自动取边界值。 double targetRps = Constants.Shooter.flywheelRpsByDistance.get(distance); - setFlywheelVelocity(targetRps, targetRps); + setFlywheelVelocity(targetRps); } public void setFlywheelVoltage(double volts) { - targetLeftFlywheelRps = 0.0; - targetRightFlywheelRps = 0.0; targetFlywheelRps = 0.0; leftFlywheel.setControl(flywheelVoltageRequest.withOutput(MathUtil.clamp(volts, -12.0, 12.0))); setRightFollowLeft(); @@ -143,8 +137,8 @@ public class ShooterSubsystem extends SubsystemBase { if (targetFlywheelRps <= 1.0) { return false; } - return Math.abs(getLeftFlywheelVelocity() - targetLeftFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps - && Math.abs(Math.abs(getRightFlywheelVelocity()) - Math.abs(targetRightFlywheelRps)) <= Constants.Shooter.flywheelSpeedToleranceRps; + return Math.abs(Math.abs(getLeftFlywheelVelocity()) - targetFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps + && Math.abs(Math.abs(getRightFlywheelVelocity()) - targetFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps; } public double getLeftFlywheelVelocity() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 27cf0b3..2fe0b06 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -17,9 +17,6 @@ import frc.robot.Constants; * shooter、intake、conveyor、底盘瞄准和 Limelight 距离如何配合， * 统一由这里决定。底盘瞄准本身仍由 DriveControls 的 AutoAimming * 模式完成，这里只读取 drive.isAimed() 等状态，不直接控制底盘电机。 - * - * 没有球检测传感器，持球状态按状态流程推断：INDEXING 完成置位， - * 射击/EJECT 后清空；跟踪不可靠时可用 Constants 里的开关旁路门控。 */ public class Superstructure extends SubsystemBase { // 操作员或自动程序请求的目标状态。 @@ -36,10 +33,7 @@ public class Superstructure extends SubsystemBase { public enum SystemState { DISABLED, STOWED, - DEPLOYING_INTAKE, INTAKING, - INDEXING, - HOLDING_NOTE, SPINNING_UP, AIMING, READY_TO_SHOOT, @@ -131,12 +125,6 @@ public class Superstructure extends SubsystemBase { return Commands.runOnce(this::requestManual, this); } - public Command toggleNotePresentCommand() { - // 持球状态是推断值，中途 disable 或漏吸球都可能让它失真； - // 给操作员一个运行时纠正入口（X 键），不用重启机器人。 - return Commands.runOnce(() -> conveyor.setNotePresent(!conveyor.hasNote())); - } - public WantedState getWantedState() { return wantedState; } @@ -150,12 +138,9 @@ public class Superstructure extends SubsystemBase { /* ====================== */ public boolean canShoot() { - boolean noteGate = !Constants.Superstructure.hasNoteGateEnabled || conveyor.hasNote(); boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary(); return shooter.atSpeed() && drive.isAimed() - && intakeRotater.atGoal() - && noteGate && stationaryGate; } @@ -194,13 +179,6 @@ public class Superstructure extends SubsystemBase { public void periodic() { // 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。 if (DriverStation.isDisabled()) { - // 进入禁用前先结算持球推断，避免中途 disable 留下失真的 hasNote： - // 喂球中被禁用视为球已交给飞轮；收纳中被禁用视为球已在传送带里。 - if (systemState == SystemState.SHOOTING) { - conveyor.setNotePresent(false); - } else if (systemState == SystemState.INDEXING) { - conveyor.setNotePresent(true); - } setSystemState(SystemState.DISABLED); stopAllMechanisms(); publishTelemetry(); @@ -220,46 +198,24 @@ public class Superstructure extends SubsystemBase { } private void handleIdle() { - // 安全默认状态：传球不动，intake 收回，背板保持受控。 + // 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。 conveyor.stop(); intakeRoller.stop(); - if (conveyor.hasNote()) { - conveyor.hold(); - } else { - conveyor.stop(); - } shooter.stopFlywheel(); shooter.stopShooterConveyor(); shooter.holdBackboardAt(0.0); } private void handleIntake() { - if (systemState != SystemState.DEPLOYING_INTAKE - && systemState != SystemState.INTAKING) { - setSystemState(SystemState.DEPLOYING_INTAKE); - } - shooter.stopFlywheel(); shooter.stopShooterConveyor(); shooter.holdBackboardAt(0.0); - intakeRotater.deployToGround(); + intakeRotater.lowerForMatch(); - if (intakeRotater.isBlockedByNotZeroed()) { - // 旋转机构未归零，安全逻辑拦下展开动作； - // 滚轮和 conveyor 也不启动，等待操作员先执行归零（A 键）。 - intakeRoller.stop(); - conveyor.stop(); - return; - } - - // 展开过程中即允许进球，位置到位后进入 INTAKING。 - // 没有球检测，收球何时结束由操作员松开按键决定（IDLE -> INDEXING）。 + // Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。 intakeRoller.intake(); conveyor.feedToShooter(); - - if (systemState == SystemState.DEPLOYING_INTAKE && intakeRotater.atGoal()) { - setSystemState(SystemState.INTAKING); - } + setSystemState(SystemState.INTAKING); } private void handlePrepShoot(boolean feedWhenReady) { @@ -268,54 +224,44 @@ public class Superstructure extends SubsystemBase { && systemState != SystemState.READY_TO_SHOOT && systemState != SystemState.SHOOTING && systemState != SystemState.CLEANUP) { - // 从 intake 流程直接转射击：没有传感器，默认球已吸入。 - // 未归零被安全逻辑拦下时滚轮从未启动，不可能有球，不置位。 - if ((systemState == SystemState.DEPLOYING_INTAKE - || systemState == SystemState.INTAKING - || systemState == SystemState.INDEXING) - && !intakeRotater.isBlockedByNotZeroed()) { - conveyor.setNotePresent(true); - } setSystemState(SystemState.SPINNING_UP); } - // 射击姿态：intake 到 HANDOFF 让出球路，滚轮低压保持球位。 - intakeRotater.moveToHandoff(); + intakeRotater.lowerForMatch(); intakeRoller.stop(); conveyor.stop(); shooter.stopShooterConveyor(); // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， // 但不会绕过 Superstructure 状态机。 double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps; - shooter.setFlywheelVelocity(targetRps, targetRps); + shooter.setFlywheelVelocity(targetRps); shooter.holdBackboardAt(Constants.Superstructure.backboardShootPosition + backboardPositionOffset); if (systemState == SystemState.SHOOTING) { // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控， // 避免球接触飞轮导致的掉速中断喂球。 + intakeRotater.shootAssist(); conveyor.feedToShooter(); shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent); if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) { - conveyor.setNotePresent(false); setSystemState(SystemState.CLEANUP); } return; } if (systemState == SystemState.CLEANUP) { + intakeRotater.lowerForMatch(); conveyor.stop(); shooter.stopShooterConveyor(); if (timeInState() > Constants.Superstructure.cleanupSeconds) { - // 不强行改写 wantedState：清理窗口内重按/按住射击键的请求不会被吞掉， - // 而 hasNote 已清空，门控自然挡住空喂球，只会保持飞轮转速待命。 - setSystemState(conveyor.hasNote() ? SystemState.HOLDING_NOTE : SystemState.STOWED); + setSystemState(SystemState.STOWED); } return; } // 未开始喂球前，球路保持静止。 - conveyor.hold(); + conveyor.stop(); shooter.stopShooterConveyor(); // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。 @@ -334,9 +280,8 @@ public class Superstructure extends SubsystemBase { private void handleEject() { // Eject 反转整条球路，飞轮保持停止。 - // 旋转机构未归零时 setGoal 内部会自行拦下，滚轮反转不受影响。 setSystemState(SystemState.EJECTING); - intakeRotater.deployToGround(); + intakeRotater.lowerForMatch(); intakeRoller.outtake(); conveyor.reverse(); shooter.runShooterConveyor(Constants.Superstructure.shooterReversePercent); @@ -382,9 +327,6 @@ public class Superstructure extends SubsystemBase { table.getEntry("driveAimed").setBoolean(drive.isAimed()); table.getEntry("driveHeadingError").setDouble(drive.getHeadingError()); table.getEntry("driveStationary").setBoolean(isDriveStationary()); - table.getEntry("intakeAtGoal").setBoolean(intakeRotater.atGoal()); - table.getEntry("intakeBlockedByNotZeroed").setBoolean(intakeRotater.isBlockedByNotZeroed()); - table.getEntry("hasNote").setBoolean(conveyor.hasNote()); table.getEntry("flywheelSpeedOffsetRps").setDouble(flywheelSpeedOffsetRps); table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset); }