From 6264ef07626e5e7e4b95ed6c41350f3b620d5cde Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 4 Jul 2026 12:19:50 +0000 Subject: [PATCH] =?UTF-8?q?=E5=AD=A3=E5=90=8E=E8=B5=9B=E9=87=8D=E6=9E=84?= =?UTF-8?q?=EF=BC=9A=E8=A1=A5=E5=AE=8C=20Superstructure=20=E7=8A=B6?= =?UTF-8?q?=E6=80=81=E6=9C=BA=E4=B8=8E=E5=B0=84=E5=87=BB=E9=97=A8=E6=8E=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Superstructure 增加 INDEXING/HOLDING_NOTE 状态，实现完整状态流转： DEPLOYING_INTAKE -> INTAKING -> INDEXING -> HOLDING_NOTE -> SPINNING_UP -> AIMING -> READY_TO_SHOOT -> SHOOTING -> CLEANUP - canShoot 门控补全：atSpeed && isAimed && rotaterAtGoal && hasNote && 底盘近静止 （hasNote/静止门控均可在 Constants 中旁路） - 无传感器持球跟踪：INDEXING 完成置位、射击/EJECT 后清空， 未归零被拦下的 intake 不会误置持球 - DriveControls 在既有 hub 方位角计算处同步 setTargetHeading， 使 isAimed 门控与 AutoAimming 实际瞄准同源；自动阶段经 pushDistanceData 保持更新 - 移除 Superstructure 基于 limelight tx 的瞄准更新（符号错误且无目标时滞留旧值） - 背板回零命令下沉到 ShooterSubsystem，OperatorControls 不再直接操作电机 - ShooterSubsystem 清理旧别名与闭环遗留接口，背板独立控制请求 - 射表改为 InterpolatingDoubleTreeMap（Constants.Shooter.flywheelRpsByDistance） - IntakeRotater：未归零时 atGoal 一律 false，新增 blockedByNotZeroed 遥测； atGoal 容差改为非零占位值 - 修复 gradlew 缺少可执行权限 Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_01YTEBmjtkz1ouCmn5yytFti --- gradlew | 0 src/main/java/frc/robot/Constants.java | 25 +- src/main/java/frc/robot/Robot.java | 3 - src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/controls/DriveControls.java | 10 +- .../frc/robot/controls/OperatorControls.java | 22 +- .../robot/subsystems/ConveyorSubsystem.java | 13 +- .../subsystems/IntakeRotaterSubsystem.java | 11 + .../robot/subsystems/LimelightSubsystem.java | 5 +- .../robot/subsystems/ShooterSubsystem.java | 131 ++++++----- .../frc/robot/subsystems/Superstructure.java | 222 ++++++++++++------ 11 files changed, 288 insertions(+), 158 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dbf71af..df9b607 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,6 +3,7 @@ package frc.robot; import com.ctre.phoenix6.configs.Slot0Configs; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; public final class Constants { private Constants() {} @@ -33,6 +34,16 @@ public final class Constants { public static final double flywheelSpeedToleranceRps = 5.0; public static final double defaultFlywheelTargetRps = 64.0; + // 距离(米) -> 飞轮转速(RPS) 射表。当前为占位点； + // 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。 + public static final InterpolatingDoubleTreeMap flywheelRpsByDistance = new InterpolatingDoubleTreeMap(); + static { + flywheelRpsByDistance.put(1.2, 58.8); + flywheelRpsByDistance.put(2.5, 64.0); + flywheelRpsByDistance.put(3.5, 68.0); + flywheelRpsByDistance.put(5.0, 74.0); + } + public static final double conveyorSpeed = 150; public static final double conveyorSpeedForAuto = 10; @@ -143,7 +154,9 @@ public final class Constants { .withKV(0.00).withKA(0.00).withKS(0.00); public static final double IntakeGetterSpeedforAuto = 0.5; - public static final double positionToleranceRotations = 0.00; + // atGoal 判定容差。为 0 会导致 atGoal 永远为 false、射击门控永远不通过； + // 这里给出可用的占位值，真车调好位置闭环后再收紧。 + public static final double positionToleranceRotations = 0.3; } public static class Conveyor { @@ -161,5 +174,15 @@ public final class Constants { public static final double shootTimeoutSeconds = 1.0; 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; + // 射击门控是否要求底盘接近静止。 + public static final boolean stationaryGateEnabled = true; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e19cca..3237263 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,12 +3,9 @@ // 修改和/或分享本代码。 package frc.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.ShooterSubsystem; public class Robot extends TimedRobot { private Command m_autonomousCommand; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17e530d..725f6f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,9 +80,9 @@ public class RobotContainer { private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain); private final GameData2026 gameData2026 = new GameData2026(); private ConfigTalonFXMotorTuner configFlywheelTuner = - new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs); + new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs); private ConfigTalonFXMotorTuner configConveyorTuner = - new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs); + new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs); private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs); diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index c07f11f..e7508aa 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -129,7 +129,11 @@ public class DriveControls { autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]); autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]); - + // 把朝向 hub 的目标角同步给底盘，供 Superstructure 读取 isAimed() 做射击门控； + // 数据来源与 AutoAimming 自动旋转完全相同，保证门控和实际瞄准一致。 + drivetrain.setTargetHeading( + drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1]))); + switch (robotStatusManager.getStatus()) { case Stopped: vx = 0; @@ -472,6 +476,10 @@ public class DriveControls { // 将距离值写入 NetworkTable autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]); autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); + // 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新， + // 让自动程序里的射击门控 isAimed() 有效。 + drivetrain.setTargetHeading( + drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1]))); } } diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index b5a3861..fbfbe6d 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -41,6 +41,10 @@ public class OperatorControls { return superstructure.requestEjectCommand(); } + public Command requestManualCommand() { + return superstructure.requestManualCommand(); + } + public void adjustFlywheelSpeedOffset(double offset) { superstructure.adjustFlywheelSpeedOffset(offset); } @@ -73,21 +77,9 @@ public class OperatorControls { ); } - // 保留旧背板归零/回零命令，方便调参和现场恢复。 + // 保留旧背板回零动作，方便调参和现场恢复； + // 具体电机操作在 ShooterSubsystem 内实现，这里只转发命令。 public Command resetBackboard0Command() { - return Commands.sequence( - Commands.runOnce(() -> shooterSubsystem.setBackboardPosition(0.0), shooterSubsystem), - Commands.run( - () -> { - if (shooterSubsystem.isBackboardAtTarget()) { - shooterSubsystem.backboardMotor.set(0.0); - } else { - shooterSubsystem.outputBackboard(); - } - }, - shooterSubsystem - ).until(shooterSubsystem::isBackboardAtTarget), - Commands.runOnce(() -> shooterSubsystem.backboardMotor.set(0.0), shooterSubsystem) - ); + return shooterSubsystem.homeBackboardCommand(); } } diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java index 99cc837..217abf8 100644 --- a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java @@ -12,7 +12,8 @@ import frc.robot.Constants; /** * 主 conveyor 的开环控制封装。 * - * 本赛季没有球检测传感器，因此这里仅报告电机状态； + * 本赛季没有球检测传感器，持球状态由 Superstructure 按状态流程推断后 + * 写入这里（INDEXING 完成置位，射击/EJECT 后清空）； * 所有动作顺序判断都放在 Superstructure。 */ public class ConveyorSubsystem extends SubsystemBase { @@ -22,6 +23,7 @@ public class ConveyorSubsystem extends SubsystemBase { private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor"); private double voltage = 0.0; + private boolean notePresent = false; public ConveyorSubsystem() { mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID); @@ -52,9 +54,18 @@ public class ConveyorSubsystem extends SubsystemBase { mainConveyor.setControl(voltageRequest.withOutput(voltage)); } + public boolean hasNote() { + return notePresent; + } + + public void setNotePresent(boolean present) { + notePresent = present; + } + @Override public void periodic() { table.getEntry("voltage").setDouble(voltage); 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 1034b57..6834aef 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java @@ -84,9 +84,19 @@ public class IntakeRotaterSubsystem extends SubsystemBase { } 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); @@ -138,6 +148,7 @@ public class IntakeRotaterSubsystem extends SubsystemBase { 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("atGoal").setBoolean(atGoal()); table.getEntry("manualVoltage").setDouble(manualVoltage); diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java index 3e4d70c..35518bc 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java @@ -48,8 +48,7 @@ public class LimelightSubsystem extends SubsystemBase { } public double getShooterSetpointByDistance() { - // 初始测试用临时映射；飞轮和背板调好后应替换为实测射击表。 - double distance = getDistanceToTarget(); - return Constants.Shooter.defaultFlywheelTargetRps + (distance - 2.5) * 4.0; + // 查 Constants 里的插值射表；真车试射后往表里加实测点即可。 + return Constants.Shooter.flywheelRpsByDistance.get(getDistanceToTarget()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9d18915..85dcc32 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -16,14 +16,18 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Encoder; +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; /** * Shooter 总成的硬件封装。 * - * 飞轮和 shooter 内部 conveyor 都使用电压开环输出。 + * 飞轮和 shooter 内部 conveyor 都使用电压开环输出， + * 右侧电机通过反向 follower 跟随左侧电机。 * 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。 + * 何时 spin up、何时喂球由 Superstructure 决定，本类不做比赛动作判断。 */ public class ShooterSubsystem extends SubsystemBase { public final TalonFX leftFlywheel; @@ -31,17 +35,13 @@ public class ShooterSubsystem extends SubsystemBase { public final TalonFX leftConveyor; public final TalonFX rightConveyor; - // 保留旧字段别名，让原有 dashboard tuner 在重构后仍能指向左飞轮和左 conveyor。 - public final TalonFX flywheelMotorLeft; - public final TalonFX conveyorMotor; - public final TalonFXS backboardMotor; public final Encoder backboardEncoder; public final ProfiledPIDController backboardPID; private final VoltageOut flywheelVoltageRequest = new VoltageOut(0); private final VoltageOut conveyorVoltageRequest = new VoltageOut(0); - private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + private final VelocityVoltage backboardVelocityRequest = new VelocityVoltage(0); private final Follower flywheelFollower; private final Follower conveyorFollower; @@ -59,9 +59,6 @@ public class ShooterSubsystem extends SubsystemBase { leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID); rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID); - flywheelMotorLeft = leftFlywheel; - conveyorMotor = leftConveyor; - leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs); rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs); leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs); @@ -95,6 +92,10 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter"); } + /* ====================== */ + /* 飞轮 */ + /* ====================== */ + public void setRightFollowLeft() { // 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。 rightFlywheel.setControl(flywheelFollower); @@ -120,16 +121,11 @@ public class ShooterSubsystem extends SubsystemBase { } public void setFlywheelVelocityByDistance(double distance) { - // 临时距离映射。拿到真实射击数据后，应替换为实测表或重新调常量。 - double clampedDistance = MathUtil.clamp(distance, 1.2, 5.0); - double targetRps = Constants.Shooter.defaultFlywheelTargetRps + (clampedDistance - 2.5) * 4.0; + // 查 Constants 里的射表，两端自动取边界值。 + double targetRps = Constants.Shooter.flywheelRpsByDistance.get(distance); setFlywheelVelocity(targetRps, targetRps); } - public void setFlywheelSpeedByRPS(double targetSpeed) { - setFlywheelVelocity(targetSpeed, targetSpeed); - } - public void setFlywheelVoltage(double volts) { targetLeftFlywheelRps = 0.0; targetRightFlywheelRps = 0.0; @@ -138,39 +134,10 @@ public class ShooterSubsystem extends SubsystemBase { setRightFollowLeft(); } - public void runShooterConveyor(double percent) { - shooterConveyorPercent = MathUtil.clamp(percent, -1.0, 1.0); - leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorPercent * 12.0)); - setRightFollowLeft(); - } - - public void stopShooterConveyor() { - runShooterConveyor(0.0); - } - - public void setConveyorSpeedByRPS(double speed) { - leftConveyor.setControl(velocityRequest.withVelocity(speed)); - setRightFollowLeft(); - } - - public void setConveyorSpeedOpen(double speed) { - runShooterConveyor(speed); - } - public void stopFlywheel() { setFlywheelVoltage(Constants.Shooter.flywheelIdleVoltage); } - public void stopAll() { - stopFlywheel(); - stopShooterConveyor(); - backboardMotor.set(0); - } - - public void stopMotors() { - stopAll(); - } - public boolean atSpeed() { // 虽然飞轮控制是电压开环，但仍读取 TalonFX 速度作为喂球门控。 if (targetFlywheelRps <= 1.0) { @@ -200,13 +167,35 @@ public class ShooterSubsystem extends SubsystemBase { return targetFlywheelRps; } + /* ====================== */ + /* shooter conveyor */ + /* ====================== */ + + public void runShooterConveyor(double percent) { + shooterConveyorPercent = MathUtil.clamp(percent, -1.0, 1.0); + leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorPercent * 12.0)); + setRightFollowLeft(); + } + + public void stopShooterConveyor() { + runShooterConveyor(0.0); + } + + public double getConveyorStatorCurrent() { + return leftConveyor.getStatorCurrent().getValueAsDouble(); + } + + /* ====================== */ + /* 背板 */ + /* ====================== */ + public void setBackboardSpeedByRPS(double speed) { if (speed > 0 && speed < 10) { speed = 10; } else if (speed < 0 && speed > -10) { speed = -10; } - backboardMotor.setControl(velocityRequest.withVelocity(speed)); + backboardMotor.setControl(backboardVelocityRequest.withVelocity(speed)); } public void setBackboardPosition(double targetPosition) { @@ -232,6 +221,20 @@ public class ShooterSubsystem extends SubsystemBase { setBackboardSpeedByRPS(pidOutput); } + public void holdBackboardAt(double targetPosition) { + // 上层只给目标位置；到位后停止输出，未到位时继续闭环。 + setBackboardPosition(targetPosition); + if (isBackboardAtTarget()) { + backboardMotor.set(0.0); + } else { + outputBackboard(); + } + } + + public void stopBackboard() { + backboardMotor.set(0.0); + } + public double getBackboardPosition() { return backboardEncoder.getDistance(); } @@ -240,28 +243,31 @@ public class ShooterSubsystem extends SubsystemBase { return backboardPID.atGoal(); } - public void conveyorWaitForAcceleration() { - stopShooterConveyor(); - } - - public void conveyorRun() { - runShooterConveyor(Constants.Superstructure.shooterFeedPercent); - } - public void resetbackboardencoder() { backboardEncoder.reset(); + backboardPID.reset(0); } - public double getConveyorStatorCurrent() { - return leftConveyor.getStatorCurrent().getValueAsDouble(); + /** + * 背板回零命令：闭环回到 0 位后停止输出。 + * 命令留在 subsystem 内，controls 层只负责绑定按键。 + */ + public Command homeBackboardCommand() { + return Commands.sequence( + Commands.runOnce(() -> setBackboardPosition(0.0), this), + Commands.run(() -> holdBackboardAt(0.0), this).until(this::isBackboardAtTarget), + Commands.runOnce(this::stopBackboard, this) + ); } - public double getDistanceToHub() { - return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0); - } + /* ====================== */ + /* 整体 */ + /* ====================== */ - public double getDistanceToPassball() { - return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0); + public void stopAll() { + stopFlywheel(); + stopShooterConveyor(); + stopBackboard(); } @Override @@ -270,6 +276,7 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("flywheelSpeed").setDouble(getAverageFlywheelVelocity()); shooterNetworkTable.getEntry("leftFlywheelSpeed").setDouble(getLeftFlywheelVelocity()); shooterNetworkTable.getEntry("rightFlywheelSpeed").setDouble(getRightFlywheelVelocity()); + shooterNetworkTable.getEntry("flywheelSpeedDifference").setDouble(getFlywheelVelocityDifference()); shooterNetworkTable.getEntry("flywheelTargetSpeed").setDouble(targetFlywheelRps); shooterNetworkTable.getEntry("flywheelAtSpeed").setBoolean(atSpeed()); shooterNetworkTable.getEntry("shooterConveyorPercent").setDouble(shooterConveyorPercent); @@ -277,8 +284,6 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition()); shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position); shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget()); - shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub()); - shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball()); shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent()); } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 520a2df..1982ff9 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,6 +1,6 @@ package frc.robot.subsystems; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; @@ -9,14 +9,17 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.subsystems.IntakeRotaterSubsystem.IntakePosition; /** * 统一协调所有得分相关机构。 * * subsystem 只暴露硬件级动作，OperatorControls 只表达操作员意图； * shooter、intake、conveyor、底盘瞄准和 Limelight 距离如何配合， - * 统一由这里决定。 + * 统一由这里决定。底盘瞄准本身仍由 DriveControls 的 AutoAimming + * 模式完成，这里只读取 drive.isAimed() 等状态，不直接控制底盘电机。 + * + * 没有球检测传感器，持球状态按状态流程推断：INDEXING 完成置位， + * 射击/EJECT 后清空；跟踪不可靠时可用 Constants 里的开关旁路门控。 */ public class Superstructure extends SubsystemBase { // 操作员或自动程序请求的目标状态。 @@ -35,6 +38,8 @@ public class Superstructure extends SubsystemBase { STOWED, DEPLOYING_INTAKE, INTAKING, + INDEXING, + HOLDING_NOTE, SPINNING_UP, AIMING, READY_TO_SHOOT, @@ -74,6 +79,10 @@ public class Superstructure extends SubsystemBase { this.limelight = limelight; } + /* ====================== */ + /* 操作员意图入口 */ + /* ====================== */ + public void requestIdle() { wantedState = WantedState.IDLE; } @@ -130,12 +139,30 @@ public class Superstructure extends SubsystemBase { return systemState; } + /* ====================== */ + /* 射击门控 */ + /* ====================== */ + public boolean canShoot() { - // 本赛季不使用球检测传感器，因此射击门控只检查机器人能实际测量的条件。 + boolean noteGate = !Constants.Superstructure.hasNoteGateEnabled || conveyor.hasNote(); + boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary(); return shooter.atSpeed() - && drive.isAimed(); + && drive.isAimed() + && intakeRotater.atGoal() + && noteGate + && stationaryGate; } + private boolean isDriveStationary() { + ChassisSpeeds speeds = drive.getChassisSpeeds(); + double translationSpeed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond; + } + + /* ====================== */ + /* 现场微调 offset */ + /* ====================== */ + public void adjustFlywheelSpeedOffset(double offsetRps) { flywheelSpeedOffsetRps += offsetRps; } @@ -153,6 +180,10 @@ public class Superstructure extends SubsystemBase { shooter.resetbackboardencoder(); } + /* ====================== */ + /* 状态机主体 */ + /* ====================== */ + @Override public void periodic() { // 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。 @@ -163,8 +194,6 @@ public class Superstructure extends SubsystemBase { return; } - updateAimingSetpoint(); - switch (wantedState) { case IDLE -> handleIdle(); case INTAKE -> handleIntake(); @@ -178,15 +207,68 @@ public class Superstructure extends SubsystemBase { } private void handleIdle() { - // 安全默认状态：传球不动，intake 收回，背板保持受控。 - conveyor.hold(); + // 从其他流程回到 IDLE 时的入口流转。 + switch (systemState) { + // 松开 intake 键后先收纳，不直接回 STOWED。 + // 未归零被安全逻辑拦下时滚轮从未启动，不可能有球，直接回 STOWED， + // 避免把 hasNote 误置为 true。 + case DEPLOYING_INTAKE, INTAKING -> setSystemState( + intakeRotater.isBlockedByNotZeroed() ? SystemState.STOWED : SystemState.INDEXING); + // 喂球中途松开按键：球已经交给飞轮，按射完处理。 + case SHOOTING -> { + conveyor.setNotePresent(false); + setSystemState(SystemState.CLEANUP); + } + // EJECT 结束：球路已排空。 + case EJECTING -> { + conveyor.setNotePresent(false); + setSystemState(SystemState.STOWED); + } + default -> {} + } + + if (systemState == SystemState.INDEXING) { + // 收纳中：intake 回 HANDOFF 交接位，滚轮低压保持，conveyor 继续送球。 + intakeRotater.moveToHandoff(); + intakeRoller.hold(); + conveyor.feedToShooter(); + shooter.stopFlywheel(); + shooter.stopShooterConveyor(); + shooter.holdBackboardAt(0.0); + + if (timeInState() > Constants.Superstructure.indexingSeconds) { + conveyor.setNotePresent(true); + setSystemState(SystemState.HOLDING_NOTE); + } + return; + } + + if (systemState == SystemState.CLEANUP) { + intakeRoller.stop(); + conveyor.stop(); + shooter.stopShooterConveyor(); + shooter.stopFlywheel(); + shooter.holdBackboardAt(0.0); + intakeRotater.stow(); + + if (timeInState() > Constants.Superstructure.cleanupSeconds) { + setSystemState(conveyor.hasNote() ? SystemState.HOLDING_NOTE : SystemState.STOWED); + } + return; + } + + // 常规空闲姿态：有球时保持球位，无球时全部停止。 + setSystemState(conveyor.hasNote() ? SystemState.HOLDING_NOTE : SystemState.STOWED); + intakeRotater.stow(); intakeRoller.stop(); - intakeRotater.stop(); + if (conveyor.hasNote()) { + conveyor.hold(); + } else { + conveyor.stop(); + } shooter.stopFlywheel(); shooter.stopShooterConveyor(); - holdBackboardAtZero(); - - setSystemState(SystemState.STOWED); + shooter.holdBackboardAt(0.0); } private void handleIntake() { @@ -197,20 +279,24 @@ public class Superstructure extends SubsystemBase { shooter.stopFlywheel(); shooter.stopShooterConveyor(); + shooter.holdBackboardAt(0.0); intakeRotater.deployToGround(); - intakeRoller.intake(); - conveyor.feedToShooter(); - holdBackboardAtZero(); - if (intakeRotater.atGoal()) { - setSystemState(SystemState.INTAKING); + if (intakeRotater.isBlockedByNotZeroed()) { + // 旋转机构未归零，安全逻辑拦下展开动作； + // 滚轮和 conveyor 也不启动，等待操作员先执行归零（A 键）。 + intakeRoller.stop(); + conveyor.stop(); + return; } - // 本赛季没有球检测，因此 intake 通过时间退出。 - // 真车确认 roller/conveyor 速度后，需要现场调整这个时间。 - if (systemState == SystemState.INTAKING && timeInState() > 1.0) { - wantedState = WantedState.IDLE; - setSystemState(SystemState.STOWED); + // 展开过程中即允许进球，位置到位后进入 INTAKING。 + // 没有球检测，收球何时结束由操作员松开按键决定（IDLE -> INDEXING）。 + intakeRoller.intake(); + conveyor.feedToShooter(); + + if (systemState == SystemState.DEPLOYING_INTAKE && intakeRotater.atGoal()) { + setSystemState(SystemState.INTAKING); } } @@ -220,63 +306,82 @@ 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) { + conveyor.setNotePresent(true); + } setSystemState(SystemState.SPINNING_UP); } + // 射击姿态：intake 到 HANDOFF 让出球路，滚轮低压保持球位。 intakeRotater.moveToHandoff(); intakeRoller.hold(); - conveyor.hold(); - shooter.stopShooterConveyor(); + // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， // 但不会绕过 Superstructure 状态机。 double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps; shooter.setFlywheelVelocity(targetRps, targetRps); - holdBackboardForShot(); - - if (shooter.atSpeed()) { - setSystemState(SystemState.AIMING); - } - - if (shooter.atSpeed() && drive.isAimed() && intakeRotater.atGoal()) { - setSystemState(SystemState.READY_TO_SHOOT); - } - - if (feedWhenReady && canShoot()) { - setSystemState(SystemState.SHOOTING); - } + shooter.holdBackboardAt(Constants.Superstructure.backboardShootPosition + backboardPositionOffset); if (systemState == SystemState.SHOOTING) { - // 只有 canShoot() 通过后才允许喂球。 + // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控， + // 避免球接触飞轮导致的掉速中断喂球。 conveyor.feedToShooter(); shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent); if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) { + conveyor.setNotePresent(false); setSystemState(SystemState.CLEANUP); } + return; } if (systemState == SystemState.CLEANUP) { conveyor.stop(); shooter.stopShooterConveyor(); if (timeInState() > Constants.Superstructure.cleanupSeconds) { + // 一次按键完成一次射击；再射需要重新请求。 wantedState = WantedState.IDLE; - setSystemState(SystemState.STOWED); + setSystemState(conveyor.hasNote() ? SystemState.HOLDING_NOTE : SystemState.STOWED); } + return; + } + + // 未开始喂球前，球路保持静止。 + conveyor.hold(); + shooter.stopShooterConveyor(); + + // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。 + if (canShoot()) { + setSystemState(SystemState.READY_TO_SHOOT); + } else if (shooter.atSpeed()) { + setSystemState(SystemState.AIMING); + } else { + setSystemState(SystemState.SPINNING_UP); + } + + if (feedWhenReady && systemState == SystemState.READY_TO_SHOOT) { + setSystemState(SystemState.SHOOTING); } } private void handleEject() { - // Eject 会反转整条球路，但飞轮保持停止。 + // Eject 反转整条球路，飞轮保持停止。 + // 旋转机构未归零时 setGoal 内部会自行拦下，滚轮反转不受影响。 setSystemState(SystemState.EJECTING); intakeRotater.deployToGround(); intakeRoller.outtake(); conveyor.reverse(); shooter.runShooterConveyor(Constants.Superstructure.shooterReversePercent); shooter.stopFlywheel(); - holdBackboardAtZero(); + shooter.holdBackboardAt(0.0); } private void handleManual() { + // 手动模式：状态机不再驱动机构，交给现场手动/调参工具； + // 只保持飞轮与喂球停止，避免意外射出。 setSystemState(SystemState.MANUAL_OVERRIDE); shooter.stopFlywheel(); shooter.stopShooterConveyor(); @@ -284,31 +389,6 @@ public class Superstructure extends SubsystemBase { intakeRoller.stop(); } - private void updateAimingSetpoint() { - // Superstructure 可以更新底盘目标朝向，但不直接下发 swerve 模块或底盘速度命令。 - if (limelight.hasTarget()) { - drive.setTargetHeading(drive.getRotation().plus(Rotation2d.fromDegrees(limelight.getTargetYaw()))); - } - } - - private void holdBackboardAtZero() { - shooter.setBackboardPosition(0.0); - if (shooter.isBackboardAtTarget()) { - shooter.backboardMotor.set(0.0); - } else { - shooter.outputBackboard(); - } - } - - private void holdBackboardForShot() { - shooter.setBackboardPosition(175.0 + backboardPositionOffset); - if (shooter.isBackboardAtTarget()) { - shooter.backboardMotor.set(0.0); - } else { - shooter.outputBackboard(); - } - } - private void stopAllMechanisms() { shooter.stopAll(); intakeRoller.stop(); @@ -328,15 +408,19 @@ public class Superstructure extends SubsystemBase { } private void publishTelemetry() { - // 发布状态和门控信号，方便判断机器人为什么还没进入射击。 + // 发布状态和各项门控信号，方便判断机器人为什么还没进入射击。 table.getEntry("wantedState").setString(wantedState.name()); table.getEntry("systemState").setString(systemState.name()); + table.getEntry("timeInState").setDouble(timeInState()); table.getEntry("canShoot").setBoolean(canShoot()); table.getEntry("shooterAtSpeed").setBoolean(shooter.atSpeed()); 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); } } -