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

subsystem修复

Browse Source
This commit is contained in:
OnDYCLaptop
2026-07-04 22:17:13 +08:00
parent 0c97452a13
commit a1fc79659e
9 changed files with 113 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/subsystems/ConveyorSubsystem.java
-8
View File
@@ -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);
}
}
src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java
+47 -99
View File
@@ -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);
}
}
src/main/java/frc/robot/subsystems/LEDSubsystem.java
+12 -5
View File
@@ -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);
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+7 -13
View File
@@ -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() {
src/main/java/frc/robot/subsystems/Superstructure.java
+11 -69
View File
@@ -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);
}