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

7.7

Browse Source
This commit is contained in:
OnDYCLaptop
2026-07-07 23:25:10 +08:00
parent 2b14e3f99e
commit 1c39a3bc42
15 changed files with 1518 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files
elastic-layout-debug.json
+1197 -198
View File
File diff suppressed because it is too large Load Diff
elastic-layout.json
+28 -28
View File
@@ -768,14 +768,14 @@
}
},
{
"title": "flywheelRunningSpeedRPS",
"title": "flywheelRunningVolts",
"x": 640.0,
"y": 128.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Tuning/flywheel/flywheelRunningSpeedRPS",
"topic": "/Tuning/flywheel/flywheelRunningVolts",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -845,27 +845,27 @@
"layouts": [],
"containers": [
{
"title": "limelight-intaker",
"title": "limelight-front",
"x": 0.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-intaker",
"topic": "/CameraPublisher/limelight-front",
"period": 0.06,
"rotation_turns": 0
}
},
{
"title": "limelight-shooter",
"title": "limelight-left",
"x": 768.0,
"y": 512.0,
"width": 640.0,
"height": 384.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-shooter",
"topic": "/CameraPublisher/limelight-left",
"period": 0.06,
"rotation_turns": 0
}
@@ -909,7 +909,7 @@
"height": 128.0,
"type": "Boolean Box",
"properties": {
"topic": "/Positioning/limelight-intaker/available",
"topic": "/Positioning/limelight-front/available",
"period": 0.06,
"data_type": "boolean",
"true_color": 4283215696,
@@ -926,7 +926,7 @@
"height": 128.0,
"type": "Boolean Box",
"properties": {
"topic": "/Positioning/limelight-shooter/available",
"topic": "/Positioning/limelight-left/available",
"period": 0.06,
"data_type": "boolean",
"true_color": 4283215696,
@@ -1034,7 +1034,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-intaker/Distance",
"topic": "/Positioning/limelight-front/Distance",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1064,7 +1064,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-shooter/Distance",
"topic": "/Positioning/limelight-left/Distance",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1138,7 +1138,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-intaker/X",
"topic": "/Positioning/limelight-front/X",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1152,7 +1152,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-intaker/Y",
"topic": "/Positioning/limelight-front/Y",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1166,7 +1166,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-shooter/X",
"topic": "/Positioning/limelight-left/X",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1180,7 +1180,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-shooter/Y",
"topic": "/Positioning/limelight-left/Y",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1195,14 +1195,14 @@
"layouts": [],
"containers": [
{
"title": "limelight-shooter",
"title": "limelight-left",
"x": 1152.0,
"y": 0.0,
"width": 384.0,
"height": 256.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-shooter",
"topic": "/CameraPublisher/limelight-left",
"period": 0.06,
"rotation_turns": 0
}
@@ -1224,14 +1224,14 @@
}
},
{
"title": "conveyorSpeed",
"title": "conveyorVelocity",
"x": 384.0,
"y": 256.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Shooter/conveyorSpeed",
"topic": "/Shooter/conveyorVelocity",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1271,14 +1271,14 @@
}
},
{
"title": "flywheelSpeed",
"title": "flywheelVelocity",
"x": 640.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/Shooter/flywheelSpeed",
"topic": "/Shooter/flywheelVelocity",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
@@ -1322,14 +1322,14 @@
}
},
{
"title": "conveyorSpeed",
"title": "conveyorVelocity",
"x": 384.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/Shooter/conveyorSpeed",
"topic": "/Shooter/conveyorVelocity",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
@@ -1338,14 +1338,14 @@
}
},
{
"title": "flywheelSpeed",
"title": "flywheelVelocity",
"x": 640.0,
"y": 256.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Shooter/flywheelSpeed",
"topic": "/Shooter/flywheelVelocity",
"period": 0.033,
"data_type": "double",
"show_submit_button": true
@@ -1380,14 +1380,14 @@
}
},
{
"title": "flywheelTargetSpeed",
"title": "flywheelTargetVoltage",
"x": 768.0,
"y": 256.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/ShooterControl/flywheelTargetSpeed",
"topic": "/Shooter/flywheelTargetVoltage",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1424,14 +1424,14 @@
}
},
{
"title": "flywheelTargetSpeed",
"title": "flywheelTargetVoltage",
"x": 640.0,
"y": 384.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/ShooterControl/flywheelTargetSpeed",
"topic": "/Shooter/flywheelTargetVoltage",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
src/main/java/frc/robot/Constants.java
+35 -38
View File
@@ -12,27 +12,28 @@ public final class Constants {
public static class Shooter {
// 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。
public static final int leftFlywheelID = 41;
public static final int rightFlywheelID = 42;
public static final int leftConveyorID = 43;
public static final int rightConveyorID = 45;
public static final int leftFlywheelID = 38;
public static final int rightFlywheelID = 39;
public static final int leftConveyorID = 40;
public static final int rightConveyorID = 37;
public static final int backboardMotorID = 44;
public static final double flywheelSpeedToleranceRps = 5.0;
public static final double defaultFlywheelTargetRps = 64.0;
public static final double defaultFlywheelVoltage = 3.34;
public static final double flywheelReadyVoltageThreshold = 1.0;
public static final double flywheelReadyDelaySeconds = 0.5;
// 距离(米) -> 飞轮转速(RPS) 射表。当前为占位点；
// 距离(米) -> 飞轮电压(V) 射表。当前为占位点；
// 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。
public static final InterpolatingDoubleTreeMap flywheelRpsByDistance = new InterpolatingDoubleTreeMap();
public static final InterpolatingDoubleTreeMap flywheelVoltageByDistance = 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);
flywheelVoltageByDistance.put(1.2, 5.82);
flywheelVoltageByDistance.put(2.5, 6.34);
flywheelVoltageByDistance.put(3.5, 6.73);
flywheelVoltageByDistance.put(5.0, 7.33);
}
public static final double conveyorSpeed = 150;
public static final double conveyorSpeedForAuto = 10;
public static final double shooterConveyorFeedVoltage = 3.0;
public static final double shooterConveyorReverseVoltage = -2.0;
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0)
@@ -47,13 +48,14 @@ public final class Constants {
.withKV(0.03).withKA(0.0).withKS(0.010);
public static class backboardPositionPID {
public static final double kP = 0.3;
public static final double kP = 0.007;
public static final double kI = 0.0;
public static final double kD = 0.0;
}
public static final double backboardSpeedMax = 60;
public static final double backboardUpLimit = 3000.0;
public static final double backboardSpeedMax = 5.0;
public static final double backboardSpeedMin = 0.4;
public static final double backboardUpLimit = 2200.0;
public static final double backboardDownLimit = 0.0;
}
@@ -93,8 +95,8 @@ public final class Constants {
}
public static class Limelight {
public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
public static final String LIMELIGHT_NAME_Shooter = "limelight-left";
public static final String LIMELIGHT_NAME_Intaker = "limelight-front";
public static final Boolean UsingMetaTag2 = true;
public static final int locatePipelineIndex = 0;
}
@@ -112,24 +114,20 @@ public final class Constants {
public static class Intaker {
// 进球机构 CAN ID 同样是占位值，真实 ID 确认后集中在这里修改。
public static final int leftIntakeRollerID = 47;
public static final int rightIntakeRollerID = 46;
public static final int leftIntakeRollerID = 20;
public static final int rightIntakeRollerID = 47;
public static final int leftIntakeRotaterID = 48;
public static final int rightIntakeRotaterID = 49;
public static final int rightIntakeRotaterID = 43;
public static final double intakeRollerIntakeSpeedRps = 30.0;
public static final double intakeRollerOuttakeSpeedRps = -30.0;
public static final double intakeRollerIntakeVoltage = -6.0;
public static final double intakeRollerOuttakeVoltage = 3.0;
public static final double intakeRotaterRaiseVoltage = 0.8;
public static final double intakeRotaterLowerVoltage = 0.05;
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 Slot0Configs intakeRollerSlot0Configs = new Slot0Configs()
.withKP(0.00).withKI(0.00).withKD(0.00)
.withKV(0.00).withKA(0.00).withKS(0.00);
@@ -141,13 +139,12 @@ public final class Constants {
public static final double IntakeGetterSpeedforAuto = 0.5;
// 这里给出可用的占位值，真车调好位置闭环后再收紧。
public static final double positionToleranceRotations = 0.3;
}
public static class Conveyor {
public static final int mainConveyorID = 50;
public static final double feedSpeedRps = 30.0;
public static final double reverseSpeedRps = -30.0;
public static final int mainConveyorID = 42;
public static final double feedVoltage = 2.5;
public static final double reverseVoltage = -2.0;
public static final Slot0Configs mainConveyorSlot0Configs = new Slot0Configs()
.withKP(0.00).withKI(0.00).withKD(0.00)
@@ -156,12 +153,12 @@ public final class Constants {
public static class Superstructure {
// 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
public static final double shooterFeedSpeedRps = Shooter.conveyorSpeed;
public static final double shooterReverseSpeedRps = -Shooter.conveyorSpeed * 0.6;
public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage;
public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage;
public static final double shootTimeoutSeconds = 1.0;
public static final double aimToleranceDegrees = 1.5;
public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
public static final double passBallFlywheelRps = Shooter.defaultFlywheelTargetRps;
public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage;
public static final double passBallBackboardPosition = 175.0;
// 射击时背板的目标位置（外接编码器计数）。
src/main/java/frc/robot/RobotContainer.java
+20 -3
View File
@@ -82,6 +82,12 @@ public class RobotContainer {
new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
private ConfigTalonFXSMotorTuner configBackboardTuner =
new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
private ConfigTalonFXMotorTuner configMainConveyorTuner =
new ConfigTalonFXMotorTuner(conveyorSubsystem.mainConveyor, "mainconveyor", Constants.Conveyor.mainConveyorSlot0Configs);
private ConfigTalonFXMotorTuner configIntakerRollerTuner =
new ConfigTalonFXMotorTuner(intakeRollerSubsystem.leftIntakeRoller, "intakerroller", Constants.Intaker.intakeRollerSlot0Configs);
private ConfigTalonFXMotorTuner configIntakerRotaterTuner =
new ConfigTalonFXMotorTuner(intakeRotaterSubsystem.leftIntakeRotater, "intakerrotater", Constants.Intaker.intakeRotaterSlot0Configs);
private final SendableChooser<Command> autoChooser;
private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
@@ -175,8 +181,8 @@ public class RobotContainer {
controllerupper.b().and(teleopOnly)
.onTrue(operatorControls.requestEjectCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(-1)));
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(1)));
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(-0.1)));
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(0.1)));
controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets()));
@@ -196,7 +202,11 @@ public class RobotContainer {
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
configConveyorTuner.periodic(null);
configBackboardTuner.periodic();
shooterSubsystem.setRightFollowLeft();
shooterSubsystem.debugBackboardTargetPeriodic();
configMainConveyorTuner.periodic(null);
configIntakerRollerTuner.periodic(null);
configIntakerRotaterTuner.periodic(null);
shooterSubsystem.setRightConveyorFollowLeft();
}
public void dashboardPeriodic() {
@@ -221,5 +231,12 @@ public class RobotContainer {
.onTrue(operatorControls.requestManualCommand())
.whileTrue(operatorControls.manualLowerIntakeRotaterCommand(intakeRotaterSubsystem))
.onFalse(operatorControls.requestIdleCommand());
controllerupper.y().and(teleopOnly)
.onTrue(Commands.sequence(
operatorControls.requestManualCommand(),
operatorControls.lowerIntakeRotaterForMatchCommand(intakeRotaterSubsystem),
operatorControls.requestIdleCommand()
));
}
}
src/main/java/frc/robot/controls/Autocommand.java
+5 -1
View File
@@ -28,7 +28,7 @@ public class Autocommand extends SequentialCommandGroup {
NamedCommands.registerCommand("start_intake_timelimit", timedIntake(superstructure));
NamedCommands.registerCommand("stop_intake", idle(superstructure));
NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater));
NamedCommands.registerCommand("intaker_up", idle(superstructure));
NamedCommands.registerCommand("intaker_up", raiseIntaker(intakeRotater));
NamedCommands.registerCommand("shoot_hub", shootHub(superstructure));
NamedCommands.registerCommand("Shake", shake(driveControls));
NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
@@ -54,6 +54,10 @@ public class Autocommand extends SequentialCommandGroup {
return intakeRotater.lowerForMatchCommand();
}
public static Command raiseIntaker(IntakeRotaterSubsystem intakeRotater) {
return intakeRotater.raiseForMatchCommand();
}
// 自动射击和手动射击使用同一套 gated shooting 流程。
public static Command shootHub(Superstructure superstructure) {
return Commands.sequence(
src/main/java/frc/robot/controls/OperatorControls.java
+6 -2
View File
@@ -50,8 +50,8 @@ public class OperatorControls {
return superstructure.requestManualCommand();
}
public void adjustFlywheelSpeedOffset(double offset) {
superstructure.adjustFlywheelSpeedOffset(offset);
public void adjustFlywheelVoltageOffset(double offset) {
superstructure.adjustFlywheelVoltageOffset(offset);
}
public void adjustBackboardRateOffset(double offset) {
@@ -74,6 +74,10 @@ public class OperatorControls {
return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater);
}
public Command lowerIntakeRotaterForMatchCommand(IntakeRotaterSubsystem intakeRotater) {
return intakeRotater.lowerForMatchCommand();
}
// PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球，
// 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。
public Command autoShootToHubCommand() {
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+2 -2
View File
@@ -81,8 +81,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 0);
positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning");
driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive");
ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-shooter");
ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker");
ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-left");
ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-front");
configueAutoBuilder();
}
src/main/java/frc/robot/subsystems/ConveyorSubsystem.java
+11 -10
View File
@@ -1,11 +1,12 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.math.MathUtil;
import frc.robot.Constants;
/**
@@ -13,31 +14,31 @@ import frc.robot.Constants;
*/
public class ConveyorSubsystem extends SubsystemBase {
public final TalonFX mainConveyor;
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
private final VoltageOut voltageRequest = new VoltageOut(0);
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
private double targetSpeedRps = 0.0;
private double targetOutputVolts = 0.0;
public ConveyorSubsystem() {
mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
mainConveyor.getConfigurator().apply(Constants.Conveyor.mainConveyorSlot0Configs);
}
public void feedToShooter() {
setSpeedByRPS(Constants.Conveyor.feedSpeedRps);
setVoltage(Constants.Conveyor.feedVoltage);
}
public void reverse() {
setSpeedByRPS(Constants.Conveyor.reverseSpeedRps);
setVoltage(Constants.Conveyor.reverseVoltage);
}
public void stop() {
setSpeedByRPS(0.0);
setVoltage(0.0);
}
public void setSpeedByRPS(double targetSpeedRps) {
this.targetSpeedRps = targetSpeedRps;
mainConveyor.setControl(velocityRequest.withVelocity(targetSpeedRps));
public void setVoltage(double outputVolts) {
targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
mainConveyor.setControl(voltageRequest.withOutput(targetOutputVolts));
}
@Override
public void periodic() {
table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
table.getEntry("targetSpeedRps").setDouble(targetSpeedRps);
table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
}
}
src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java
+12 -11
View File
@@ -1,10 +1,11 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
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.wpilibj2.command.SubsystemBase;
@@ -17,13 +18,13 @@ import frc.robot.Constants;
* 本类只负责成对 TalonFX 输出和 follower 关系。
*/
public class IntakeRollerSubsystem extends SubsystemBase {
private final TalonFX leftIntakeRoller;
public final TalonFX leftIntakeRoller;
private final TalonFX rightIntakeRoller;
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
private final VoltageOut voltageRequest = new VoltageOut(0);
private final Follower rightFollower;
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller");
private double targetSpeedRps = 0.0;
private double targetOutputVolts = 0.0;
public IntakeRollerSubsystem() {
leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID);
@@ -35,20 +36,20 @@ public class IntakeRollerSubsystem extends SubsystemBase {
}
public void intake() {
setSpeedByRPS(Constants.Intaker.intakeRollerIntakeSpeedRps);
setVoltage(Constants.Intaker.intakeRollerIntakeVoltage);
}
public void outtake() {
setSpeedByRPS(Constants.Intaker.intakeRollerOuttakeSpeedRps);
setVoltage(Constants.Intaker.intakeRollerOuttakeVoltage);
}
public void stop() {
setSpeedByRPS(0.0);
setVoltage(0.0);
}
public void setSpeedByRPS(double targetSpeedRps) {
this.targetSpeedRps = targetSpeedRps;
leftIntakeRoller.setControl(velocityRequest.withVelocity(targetSpeedRps));
public void setVoltage(double outputVolts) {
targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
leftIntakeRoller.setControl(voltageRequest.withOutput(targetOutputVolts));
followLeft();
}
@@ -67,6 +68,6 @@ public class IntakeRollerSubsystem extends SubsystemBase {
table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble());
table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble());
table.getEntry("velocityDifference").setDouble(getVelocityDifference());
table.getEntry("targetSpeedRps").setDouble(targetSpeedRps);
table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
}
}
src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java
+45 -27
View File
@@ -1,7 +1,6 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
@@ -18,19 +17,17 @@ import frc.robot.Constants;
/**
* Intaker rotation control for the 2026 fuel path.
*
* The normal match posture is lowered. During shooting, the arm alternates
* between lowered and an upward assist position to help fuel enter the chamber.
* The rotater uses timed voltage actions only. Position is published for
* telemetry, but it is not used for closed-loop control.
*/
public class IntakeRotaterSubsystem extends SubsystemBase {
private final TalonFX leftIntakeRotater;
public 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 double targetPosition = Constants.Intaker.intakeRotaterLoweredPosition;
private double outputVoltage = 0.0;
public IntakeRotaterSubsystem() {
@@ -46,51 +43,70 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
}
public void lowerForMatch() {
setPosition(Constants.Intaker.intakeRotaterLoweredPosition);
setVoltage(0.05);
}
public Command lowerForMatchCommand() {
return Commands.runOnce(this::lowerForMatch, this);
return Commands.sequence(
Commands.runOnce(() -> setVoltage(-1.5), this),
Commands.waitSeconds(0.7),
Commands.runOnce(() -> setVoltage(0.05), this),
Commands.waitSeconds(0.7),
Commands.runOnce(this::stop, this)
);
}
public void raiseForMatch() {
setVoltage(0.8);
}
public Command raiseForMatchCommand() {
return Commands.sequence(
Commands.runOnce(this::raiseForMatch, this),
Commands.waitSeconds(0.5),
Commands.runOnce(this::stop, this)
);
}
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);
double volts = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle
? Constants.Intaker.intakeRotaterRaiseVoltage
: Constants.Intaker.intakeRotaterLowerVoltage;
setVoltage(volts);
}
public void manualRaise() {
setVoltage(Constants.Intaker.intakeRotaterManualRaiseVoltage);
setVoltage(Constants.Intaker.intakeRotaterRaiseVoltage);
}
public void manualLower() {
setVoltage(Constants.Intaker.intakeRotaterManualLowerVoltage);
setVoltage(Constants.Intaker.intakeRotaterLowerVoltage);
}
public void stop() {
setVoltage(0.0);
}
public boolean atGoal() {
return Math.abs(getPosition() - targetPosition) <= Constants.Intaker.positionToleranceRotations;
public boolean isRunning() {
return Math.abs(outputVoltage) > 1e-6;
}
public double getPosition() {
return leftIntakeRotater.getPosition().getValueAsDouble();
}
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 double getLeftVelocity() {
return leftIntakeRotater.getVelocity().getValueAsDouble();
}
public double getRightVelocity() {
return rightIntakeRotater.getVelocity().getValueAsDouble();
}
private double getVelocityDifference() {
return Math.abs(getLeftVelocity()) - Math.abs(getRightVelocity());
}
private void setVoltage(double volts) {
@@ -106,8 +122,10 @@ public class IntakeRotaterSubsystem extends SubsystemBase {
@Override
public void periodic() {
table.getEntry("position").setDouble(getPosition());
table.getEntry("targetPosition").setDouble(targetPosition);
table.getEntry("atGoal").setBoolean(atGoal());
table.getEntry("isRunning").setBoolean(isRunning());
table.getEntry("outputVoltage").setDouble(outputVoltage);
table.getEntry("leftVelocity").setDouble(getLeftVelocity());
table.getEntry("rightVelocity").setDouble(getRightVelocity());
table.getEntry("velocityDifference").setDouble(getVelocityDifference());
}
}
src/main/java/frc/robot/subsystems/LimelightSubsystem.java
+2 -2
View File
@@ -47,8 +47,8 @@ public class LimelightSubsystem extends SubsystemBase {
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
}
public double getShooterSetpointByDistance() {
public double getShooterVoltageByDistance() {
// 查 Constants 里的插值射表；真车试射后往表里加实测点即可。
return Constants.Shooter.flywheelRpsByDistance.get(getDistanceToTarget());
return Constants.Shooter.flywheelVoltageByDistance.get(getDistanceToTarget());
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+129 -65
View File
@@ -3,7 +3,7 @@ package frc.robot.subsystems;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.signals.InvertedValue;
@@ -15,6 +15,7 @@ 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.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -23,8 +24,8 @@ import frc.robot.Constants;
/**
* Shooter 总成的硬件封装。
*
* 飞轮和 shooter 内部 conveyor 都使用电压开环输出，
* 右侧电机通过反向 follower 跟随左侧电机。
* 飞轮和 shooter 内部 conveyor 使用 VoltageOut 开环电压输出；
* shooter 内部 conveyor 仍由右侧电机反向 follower 跟随左侧电机。
* 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。
* 何时 spin up、何时喂球由 Superstructure 决定，本类不做比赛动作判断。
*/
@@ -38,17 +39,24 @@ public class ShooterSubsystem extends SubsystemBase {
public final Encoder backboardEncoder;
public final ProfiledPIDController backboardPID;
private final VelocityVoltage flywheelVelocityRequest = new VelocityVoltage(0);
private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0);
private final VelocityVoltage backboardVelocityRequest = new VelocityVoltage(0);
private final VoltageOut leftFlywheelVoltageRequest = new VoltageOut(0);
private final VoltageOut rightFlywheelVoltageRequest = new VoltageOut(0);
private final VoltageOut conveyorVoltageRequest = new VoltageOut(0);
private final VoltageOut backboardVoltageRequest = new VoltageOut(0);
private final Follower flywheelFollower;
private final Follower conveyorFollower;
private final NetworkTable shooterNetworkTable;
private final NetworkTable backboardTuningTable;
private double targetFlywheelRps = 0.0;
private double shooterConveyorTargetRps = 0.0;
private double flywheelOutputVolts = 0.0;
private double flywheelVoltageStartTimestamp = 0.0;
private double shooterConveyorOutputVolts = 0.0;
private double backboardOutputVolts = 0.0;
private double backboardRawPidOutputVolts = 0.0;
private double backboardClampedPidOutputVolts = 0.0;
private boolean backboardOutputSaturated = false;
private boolean backboardMinimumOutputActive = false;
public ShooterSubsystem() {
leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID);
@@ -66,9 +74,8 @@ public class ShooterSubsystem extends SubsystemBase {
leftFlywheel.getConfigurator().apply(motorOutputConfigs);
leftConveyor.getConfigurator().apply(motorOutputConfigs);
flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
setRightFollowLeft();
setRightConveyorFollowLeft();
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
@@ -87,15 +94,17 @@ public class ShooterSubsystem extends SubsystemBase {
backboardPID.setTolerance(10.0);
shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
}
/* ====================== */
/* 飞轮 */
/* ====================== */
public void setRightFollowLeft() {
public void setRightConveyorFollowLeft() {
// 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
rightFlywheel.setControl(flywheelFollower);
rightConveyor.setControl(conveyorFollower);
}
@@ -105,37 +114,32 @@ public class ShooterSubsystem extends SubsystemBase {
rightFlywheel.getConfigurator().apply(configs);
}
public void setFlywheelVelocity(double targetRps) {
// 右飞轮是 follower，因此对外只暴露一个飞轮目标速度。
// 左飞轮使用 VelocityVoltage 速度闭环，右飞轮反向跟随左飞轮。
setFlywheelSpeedByRPS(targetRps);
public void setFlywheelVoltage(double outputVolts) {
double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
if (Math.abs(clampedOutputVolts - flywheelOutputVolts) > 1e-6) {
flywheelVoltageStartTimestamp = Timer.getFPGATimestamp();
}
flywheelOutputVolts = clampedOutputVolts;
leftFlywheel.setControl(leftFlywheelVoltageRequest.withOutput(flywheelOutputVolts));
rightFlywheel.setControl(rightFlywheelVoltageRequest.withOutput(-flywheelOutputVolts));
}
public void setFlywheelSpeedByRPS(double targetRps) {
targetFlywheelRps = Math.abs(targetRps);
leftFlywheel.setControl(flywheelVelocityRequest.withVelocity(targetFlywheelRps));
setRightFollowLeft();
}
public void setFlywheelVelocityByDistance(double distance) {
// 查 Constants 里的射表，两端自动取边界值。
double targetRps = Constants.Shooter.flywheelRpsByDistance.get(distance);
setFlywheelVelocity(targetRps);
public void setFlywheelVoltageByDistance(double distance) {
double targetVolts = Constants.Shooter.flywheelVoltageByDistance.get(distance);
setFlywheelVoltage(targetVolts);
}
public void stopFlywheel() {
setFlywheelSpeedByRPS(0.0);
setFlywheelVoltage(0.0);
}
public boolean atSpeed() {
// 虽然飞轮控制是电压开环，但仍读取 TalonFX 速度作为喂球门控。
if (targetFlywheelRps <= 1.0) {
public boolean atVoltage() {
if (Math.abs(flywheelOutputVolts) < Constants.Shooter.flywheelReadyVoltageThreshold) {
return false;
}
return Math.abs(Math.abs(getLeftFlywheelVelocity()) - targetFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps
&& Math.abs(Math.abs(getRightFlywheelVelocity()) - targetFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps;
return Timer.getFPGATimestamp() - flywheelVoltageStartTimestamp
>= Constants.Shooter.flywheelReadyDelaySeconds;
}
public double getLeftFlywheelVelocity() {
return leftFlywheel.getVelocity().getValueAsDouble();
}
@@ -152,22 +156,22 @@ public class ShooterSubsystem extends SubsystemBase {
return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
}
public double getTargetFlywheelRps() {
return targetFlywheelRps;
public double getTargetFlywheelVoltage() {
return flywheelOutputVolts;
}
/* ====================== */
/* shooter conveyor */
/* ====================== */
public void runShooterConveyorByRPS(double targetRps) {
shooterConveyorTargetRps = targetRps;
leftConveyor.setControl(conveyorVelocityRequest.withVelocity(targetRps));
setRightFollowLeft();
public void runShooterConveyorVoltage(double outputVolts) {
shooterConveyorOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorOutputVolts));
setRightConveyorFollowLeft();
}
public void stopShooterConveyor() {
runShooterConveyorByRPS(0.0);
runShooterConveyorVoltage(0.0);
}
public double getConveyorStatorCurrent() {
@@ -178,13 +182,13 @@ public class ShooterSubsystem extends SubsystemBase {
/* 背板 */
/* ====================== */
public void setBackboardSpeedByRPS(double speed) {
if (speed > 0 && speed < 10) {
speed = 10;
} else if (speed < 0 && speed > -10) {
speed = -10;
}
backboardMotor.setControl(backboardVelocityRequest.withVelocity(speed));
public void setBackboardVoltage(double outputVolts) {
backboardOutputVolts = MathUtil.clamp(
outputVolts,
-Constants.Shooter.backboardSpeedMax,
Constants.Shooter.backboardSpeedMax
);
backboardMotor.setControl(backboardVoltageRequest.withOutput(backboardOutputVolts));
}
public void setBackboardPosition(double targetPosition) {
@@ -199,29 +203,47 @@ public class ShooterSubsystem extends SubsystemBase {
public void outputBackboard() {
// 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
double pidOutput = backboardPID.calculate(getBackboardPosition());
pidOutput = MathUtil.clamp(pidOutput, -Constants.Shooter.backboardSpeedMax, Constants.Shooter.backboardSpeedMax);
double rawPidOutput = backboardPID.calculate(getBackboardPosition());
double pidOutput = MathUtil.clamp(
rawPidOutput,
-Constants.Shooter.backboardSpeedMax,
Constants.Shooter.backboardSpeedMax
);
backboardRawPidOutputVolts = rawPidOutput;
backboardClampedPidOutputVolts = pidOutput;
backboardOutputSaturated = Math.abs(rawPidOutput - pidOutput) > 1e-9;
if (Math.abs(pidOutput) < 0.8) {
pidOutput = 0.0;
if (pidOutput != 0.0 && Math.abs(pidOutput) < Constants.Shooter.backboardSpeedMin) {
pidOutput = Math.copySign(Constants.Shooter.backboardSpeedMin, pidOutput);
backboardMinimumOutputActive = true;
} else {
backboardMinimumOutputActive = false;
}
shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
setBackboardSpeedByRPS(pidOutput);
setBackboardVoltage(pidOutput);
}
public void holdBackboardAt(double targetPosition) {
// 上层只给目标位置；到位后停止输出，未到位时继续闭环。
setBackboardPosition(targetPosition);
if (isBackboardAtTarget()) {
backboardMotor.set(0.0);
backboardRawPidOutputVolts = 0.0;
backboardClampedPidOutputVolts = 0.0;
backboardOutputSaturated = false;
backboardMinimumOutputActive = false;
setBackboardVoltage(0.0);
} else {
outputBackboard();
}
}
public void stopBackboard() {
backboardMotor.set(0.0);
backboardRawPidOutputVolts = 0.0;
backboardClampedPidOutputVolts = 0.0;
backboardOutputSaturated = false;
backboardMinimumOutputActive = false;
setBackboardVoltage(0.0);
}
public double getBackboardPosition() {
@@ -259,20 +281,62 @@ public class ShooterSubsystem extends SubsystemBase {
stopBackboard();
}
public void debugBackboardTargetPeriodic() {
boolean positionEnable = backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0);
double clampedTarget = MathUtil.clamp(
requestedTarget,
Constants.Shooter.backboardDownLimit,
Constants.Shooter.backboardUpLimit
);
backboardTuningTable.getEntry("backboardPositionEnableActive").setBoolean(positionEnable);
backboardTuningTable.getEntry("backboardCommandedTargetPosition").setDouble(requestedTarget);
backboardTuningTable.getEntry("backboardClampedTargetPosition").setDouble(clampedTarget);
if (positionEnable) {
holdBackboardAt(clampedTarget);
}
}
@Override
public void periodic() {
backboardPID.calculate(getBackboardPosition());
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("shooterConveyorTargetRps").setDouble(shooterConveyorTargetRps);
shooterNetworkTable.getEntry("conveyorSpeed").setDouble(leftConveyor.getVelocity().getValueAsDouble());
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
double backboardPosition = getBackboardPosition();
double backboardGoalPosition = backboardPID.getGoal().position;
double backboardSetpointPosition = backboardPID.getSetpoint().position;
shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity());
shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference());
shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage());
shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts);
shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage());
shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts);
shooterNetworkTable.getEntry("conveyorVelocity").setDouble(leftConveyor.getVelocity().getValueAsDouble());
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);
shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
shooterNetworkTable.getEntry("backboardGoalPosition").setDouble(backboardGoalPosition);
shooterNetworkTable.getEntry("backboardSetpointPosition").setDouble(backboardSetpointPosition);
shooterNetworkTable.getEntry("backboardSetpointVelocity").setDouble(backboardPID.getSetpoint().velocity);
shooterNetworkTable.getEntry("backboardGoalError").setDouble(backboardGoalPosition - backboardPosition);
shooterNetworkTable.getEntry("backboardSetpointError").setDouble(backboardSetpointPosition - backboardPosition);
shooterNetworkTable.getEntry("backboardRawPIDOutput").setDouble(backboardRawPidOutputVolts);
shooterNetworkTable.getEntry("backboardClampedPIDOutput").setDouble(backboardClampedPidOutputVolts);
shooterNetworkTable.getEntry("backboardOutputSaturated").setBoolean(backboardOutputSaturated);
shooterNetworkTable.getEntry("backboardMinimumOutputActive").setBoolean(backboardMinimumOutputActive);
shooterNetworkTable.getEntry("backboardMaxOutputVolts").setDouble(Constants.Shooter.backboardSpeedMax);
shooterNetworkTable.getEntry("backboardMinOutputVolts").setDouble(Constants.Shooter.backboardSpeedMin);
shooterNetworkTable.getEntry("backboardPositionTolerance").setDouble(10.0);
shooterNetworkTable.getEntry("backboardShootPosition").setDouble(Constants.Superstructure.backboardShootPosition);
shooterNetworkTable.getEntry("backboardPassPosition").setDouble(Constants.Superstructure.passBallBackboardPosition);
shooterNetworkTable.getEntry("backboardMotorPosition").setDouble(backboardMotor.getPosition().getValueAsDouble());
shooterNetworkTable.getEntry("backboardMotorVelocity").setDouble(backboardMotor.getVelocity().getValueAsDouble());
shooterNetworkTable.getEntry("backboardMotorVoltage").setDouble(backboardMotor.getMotorVoltage().getValueAsDouble());
shooterNetworkTable.getEntry("backboardMotorStatorCurrent").setDouble(backboardMotor.getStatorCurrent().getValueAsDouble());
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
}
}
src/main/java/frc/robot/subsystems/Superstructure.java
+16 -15
View File
@@ -52,7 +52,7 @@ public class Superstructure extends SubsystemBase {
private WantedState wantedState = WantedState.IDLE;
private SystemState systemState = SystemState.STOWED;
private double stateStartTimestamp = Timer.getFPGATimestamp();
private double flywheelSpeedOffsetRps = 0.0;
private double flywheelVoltageOffset = 0.0;
private double backboardPositionOffset = 0.0;
private boolean shotCompletedThisRequest = false;
@@ -164,7 +164,7 @@ public class Superstructure extends SubsystemBase {
public boolean canShoot() {
boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary();
return shooter.atSpeed()
return shooter.atVoltage()
&& drive.isAimed()
&& stationaryGate;
}
@@ -179,8 +179,8 @@ public class Superstructure extends SubsystemBase {
/* 现场微调 offset */
/* ====================== */
public void adjustFlywheelSpeedOffset(double offsetRps) {
flywheelSpeedOffsetRps += offsetRps;
public void adjustFlywheelVoltageOffset(double offsetVolts) {
flywheelVoltageOffset += offsetVolts;
}
public void adjustBackboardPositionOffset(double offset) {
@@ -188,7 +188,7 @@ public class Superstructure extends SubsystemBase {
}
public void resetOffsets() {
flywheelSpeedOffsetRps = 0.0;
flywheelVoltageOffset = 0.0;
backboardPositionOffset = 0.0;
}
@@ -227,6 +227,7 @@ public class Superstructure extends SubsystemBase {
// 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。
conveyor.stop();
intakeRoller.stop();
intakeRotater.stop();
shooter.stopFlywheel();
shooter.stopShooterConveyor();
shooter.holdBackboardAt(0.0);
@@ -253,18 +254,18 @@ public class Superstructure extends SubsystemBase {
}
private void handleShootHub() {
double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
double targetVolts = limelight.getShooterVoltageByDistance() + flywheelVoltageOffset;
double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
handlePreparedFeed(targetRps, backboardPosition);
handlePreparedFeed(targetVolts, backboardPosition);
}
private void handlePassBall() {
double targetRps = Constants.Superstructure.passBallFlywheelRps + flywheelSpeedOffsetRps;
double targetVolts = Constants.Superstructure.passBallFlywheelVoltage + flywheelVoltageOffset;
double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
handlePreparedFeed(targetRps, backboardPosition);
handlePreparedFeed(targetVolts, backboardPosition);
}
private void handlePreparedFeed(double targetRps, double backboardPosition) {
private void handlePreparedFeed(double targetVolts, double backboardPosition) {
if (systemState != SystemState.PREP_SHOOT
&& systemState != SystemState.SHOOTING) {
setSystemState(SystemState.PREP_SHOOT);
@@ -274,7 +275,7 @@ public class Superstructure extends SubsystemBase {
shooter.stopShooterConveyor();
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
// 但不会绕过 Superstructure 状态机。
shooter.setFlywheelVelocity(targetRps);
shooter.setFlywheelVoltage(targetVolts);
shooter.holdBackboardAt(backboardPosition);
if (systemState == SystemState.SHOOTING) {
@@ -282,7 +283,7 @@ public class Superstructure extends SubsystemBase {
// 避免球接触飞轮导致的掉速中断喂球。
intakeRotater.shootAssist();
conveyor.feedToShooter();
shooter.runShooterConveyorByRPS(Constants.Superstructure.shooterFeedSpeedRps);
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterFeedVoltage);
if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
shotCompletedThisRequest = true;
@@ -306,7 +307,7 @@ public class Superstructure extends SubsystemBase {
setSystemState(SystemState.EJECTING);
intakeRoller.outtake();
conveyor.reverse();
shooter.runShooterConveyorByRPS(Constants.Superstructure.shooterReverseSpeedRps);
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage);
shooter.stopFlywheel();
shooter.holdBackboardAt(0.0);
}
@@ -345,11 +346,11 @@ public class Superstructure extends SubsystemBase {
table.getEntry("systemState").setString(systemState.name());
table.getEntry("timeInState").setDouble(timeInState());
table.getEntry("canShoot").setBoolean(canShoot());
table.getEntry("shooterAtSpeed").setBoolean(shooter.atSpeed());
table.getEntry("shooterAtVoltage").setBoolean(shooter.atVoltage());
table.getEntry("driveAimed").setBoolean(drive.isAimed());
table.getEntry("driveHeadingError").setDouble(drive.getHeadingError());
table.getEntry("driveStationary").setBoolean(isDriveStationary());
table.getEntry("flywheelSpeedOffsetRps").setDouble(flywheelSpeedOffsetRps);
table.getEntry("flywheelVoltageOffset").setDouble(flywheelVoltageOffset);
table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
}
}
src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java
+10 -9
View File
@@ -2,8 +2,9 @@ package frc.robot.tuning;
import java.util.function.Function;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.networktables.*;
public class ConfigTalonFXMotorTuner {
@@ -22,10 +23,10 @@ public class ConfigTalonFXMotorTuner {
private final DoubleSubscriber KsSub;
private final BooleanSubscriber enableRuningSub;
private final DoubleSubscriber runningSpeedRPS;
private final DoubleSubscriber runningVolts;
private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
private final VoltageOut voltageRequest = new VoltageOut(0);
public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
this.motor = motor_;
this.motorName = motorName_;
@@ -42,7 +43,7 @@ public class ConfigTalonFXMotorTuner {
table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);;
table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);;
table.getDoubleTopic(motorName+"RunningVolts").publish().set(0);;
this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
@@ -52,7 +53,7 @@ public class ConfigTalonFXMotorTuner {
this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA);
this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS);
this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false);
this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0);
this.runningVolts = table.getDoubleTopic(motorName+"RunningVolts").subscribe(0);
}
public void periodic(Runnable func) {
@@ -80,11 +81,11 @@ public class ConfigTalonFXMotorTuner {
}
if (enableRuningSub.get()) {;
double speed = runningSpeedRPS.get();
motor.setControl(velocityRequest.withVelocity(speed));
// System.out.println("running motor with speed: "+speed+" on motor: "+motorName);
double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0);
motor.setControl(voltageRequest.withOutput(volts));
// System.out.println("running motor with volts: "+volts+" on motor: "+motorName);
}else{
motor.setControl(velocityRequest.withVelocity(0));
motor.setControl(voltageRequest.withOutput(0));
}
}
src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java
-1
View File
@@ -1,7 +1,6 @@
package frc.robot.tuning;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFXS;
import edu.wpi.first.networktables.*;