admin6494/FRC6494_2026Regional-offseason
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #2 from vic233333/claude/frc-postseason-refactor-wepjot

Browse Source 
Claude/frc postseason refactor wepjot
This commit is contained in:
dingdoorfiah-ui
2026-07-04 20:56:05 +08:00
committed by GitHub
parent 3c5d17b527 9da20a108c
commit fad52c40c0
11 changed files with 338 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gradlew
Vendored Regular → Executable
View File
src/main/java/frc/robot/Constants.java
+35 -1
View File
@@ -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,16 @@ 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;
// 上电时按机构处于收拢（STOW）姿态直接归零。
// 比赛规则要求开赛前机器人处于合法初始姿态，因此这是安全假设；
// 若维修后姿态不确定，置 false 并用操作员 A 键手动归零。
// 没有这个假设，自动赛阶段无人能按归零键，收球和射击会被
// 未归零保护永久拦下。
public static final boolean assumeZeroedOnBoot = true;
}
public static class Conveyor {
@@ -161,5 +181,19 @@ 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;
// 比赛开局机器人带预装球，开机默认持球为 true；
// 否则自动赛的第一发射击会被 hasNote 门控拦死。
// 空车调试时可置 false，或用操作员 X 键运行时翻转持球状态。
public static final boolean assumePreloadedAtBoot = true;
// 射击门控是否要求底盘接近静止。
public static final boolean stationaryGateEnabled = true;
}
}
src/main/java/frc/robot/Robot.java
-3
View File
@@ -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;
src/main/java/frc/robot/RobotContainer.java
+11 -6
View File
@@ -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);
@@ -175,15 +175,20 @@ public class RobotContainer {
private void configueShooter() {
// 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure
// 根据飞轮速度、底盘瞄准和进球旋转机构位置统一判断。
controllerupper.rightTrigger()
// 这些命令 require Superstructure，与自动程序共用需求；
// 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
Trigger teleopOnly = RobotModeTriggers.teleop();
controllerupper.rightTrigger().and(teleopOnly)
.onTrue(operatorControls.requestShootCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.y()
controllerupper.y().and(teleopOnly)
.onTrue(operatorControls.requestPrepShootCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.b()
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)));
@@ -215,7 +220,7 @@ public class RobotContainer {
private void configueIntaker() {
// 进球相关按钮也只请求 Superstructure 状态。
// A 键是校准动作，只应在进球机构处于已知零点姿态时按下。
controllerupper.leftTrigger()
controllerupper.leftTrigger().and(RobotModeTriggers.teleop())
.onTrue(operatorControls.requestIntakeCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.a().onTrue(operatorControls.resetIntakeRotaterEncoderCommand(intakeRotaterSubsystem));
src/main/java/frc/robot/controls/DriveControls.java
+9 -1
View File
@@ -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])));
}
}
src/main/java/frc/robot/controls/OperatorControls.java
+11 -15
View File
@@ -41,6 +41,14 @@ public class OperatorControls {
return superstructure.requestEjectCommand();
}
public Command requestManualCommand() {
return superstructure.requestManualCommand();
}
public Command toggleNotePresentCommand() {
return superstructure.toggleNotePresentCommand();
}
public void adjustFlywheelSpeedOffset(double offset) {
superstructure.adjustFlywheelSpeedOffset(offset);
}
@@ -73,21 +81,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();
}
}
src/main/java/frc/robot/subsystems/ConveyorSubsystem.java
+13 -1
View File
@@ -12,7 +12,8 @@ import frc.robot.Constants;
/**
* 主 conveyor 的开环控制封装。
*
* 本赛季没有球检测传感器，因此这里仅报告电机状态；
* 本赛季没有球检测传感器，持球状态由 Superstructure 按状态流程推断后
* 写入这里（INDEXING 完成置位，射击/EJECT 后清空）；
* 所有动作顺序判断都放在 Superstructure。
*/
public class ConveyorSubsystem extends SubsystemBase {
@@ -22,6 +23,8 @@ public class ConveyorSubsystem extends SubsystemBase {
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
private double voltage = 0.0;
// 开局带预装球时默认持球，否则自动赛第一发会被 hasNote 门控拦死。
private boolean notePresent = Constants.Superstructure.assumePreloadedAtBoot;
public ConveyorSubsystem() {
mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
@@ -52,9 +55,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);
}
}
src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java
+20
View File
@@ -51,6 +51,15 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
rightIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
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;
}
}
public void setGoal(IntakePosition position) {
@@ -84,9 +93,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 +157,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);
src/main/java/frc/robot/subsystems/LimelightSubsystem.java
+2 -3
View File
@@ -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());
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+68 -63
View File
@@ -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());
}
}
src/main/java/frc/robot/subsystems/Superstructure.java
+169 -70
View File
@@ -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;
}
@@ -122,6 +131,12 @@ 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;
}
@@ -130,12 +145,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,18 +186,27 @@ public class Superstructure extends SubsystemBase {
shooter.resetbackboardencoder();
}
/* ====================== */
/* 状态机主体 */
/* ====================== */
@Override
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();
return;
}
updateAimingSetpoint();
switch (wantedState) {
case IDLE -> handleIdle();
case INTAKE -> handleIntake();
@@ -178,15 +220,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 +292,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 +319,84 @@ 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();
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);
// 不强行改写 wantedState：清理窗口内重按/按住射击键的请求不会被吞掉，
// 而 hasNote 已清空，门控自然挡住空喂球，只会保持飞轮转速待命。
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 +404,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 +423,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);
}
}