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

7.10晚

Browse Source
This commit is contained in:
OnDYCLaptop
2026-07-10 22:10:06 +08:00
parent 6abdf34cf0
commit d2f5adc813
10 changed files with 94 additions and 1022 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/Constants.java
+4 -18
View File
@@ -27,22 +27,9 @@ public final class Constants {
public static final double flywheelReadyDelaySeconds = 0.5;
// Distance (meters) -> flywheel velocity (rotations per second).
public static final InterpolatingDoubleTreeMap flywheelVelocityByDistance = new InterpolatingDoubleTreeMap();
static {
flywheelVelocityByDistance.put(1.2, 2.0 / flywheelNominalKv);
flywheelVelocityByDistance.put(2.5, 2.1 / flywheelNominalKv);
flywheelVelocityByDistance.put(3.5, 2.2 / flywheelNominalKv);
flywheelVelocityByDistance.put(5.0, 2.3 / flywheelNominalKv);
}
public static final double shooterConveyorFeedVoltage = 3.0;
public static final double shooterConveyorReverseVoltage = -2.0;
public static final double shooterConveyorNominalKv = 0.125;
public static final double shooterConveyorMaxVelocity = 12.0 / shooterConveyorNominalKv;
public static final double shooterConveyorFeedVelocity =
shooterConveyorFeedVoltage / shooterConveyorNominalKv;
public static final double shooterConveyorReverseVelocity =
shooterConveyorReverseVoltage / shooterConveyorNominalKv;
public static final double shooterConveyorFeedVelocity = 40;
public static final double shooterConveyorReverseVelocity = -10;
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.21)
@@ -54,7 +41,7 @@ public final class Constants {
public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
.withKP(0.19).withKI(0).withKD(0.001)
.withKV(shooterConveyorNominalKv).withKA(0.0).withKS(0.07);
.withKV(0.125).withKA(0.0).withKS(0.07);
public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
.withKP(0.01).withKI(0).withKD(0)
@@ -168,8 +155,7 @@ public final class Constants {
public static class Superstructure {
// State-machine timing and gate tolerances; tune on the real robot.
public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage;
public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage;
public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity;
public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity;
public static final double shootTimeoutSeconds = 1.0;
src/main/java/frc/robot/RobotContainer.java
+24 -72
View File
@@ -36,9 +36,8 @@ public class RobotContainer {
/* 手柄 */
/* ====================== */
// 使用两个 Xbox 手柄：0 号给底盘驾驶，1 号给机构操作员。
private final CommandXboxController controllerlower = new CommandXboxController(0);
private final CommandXboxController controllerupper = new CommandXboxController(1);
// 使用一个 Xbox 手柄：0 号口同时负责底盘驾驶和机构操作。
private final CommandXboxController controller = new CommandXboxController(0);
/* ====================== */
/* 子系统 */
@@ -70,7 +69,7 @@ public class RobotContainer {
/* 控制封装/调参 */
/* ====================== */
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, superstructure);
public final DriveControls driveControls = new DriveControls(drivetrain, controller, superstructure);
// 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
@@ -78,16 +77,8 @@ public class RobotContainer {
private final GameData2026 gameData2026 = new GameData2026();
private ConfigTalonFXMotorTuner configFlywheelTuner =
new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs, false);
private ConfigTalonFXMotorTuner configConveyorTuner =
new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs, false);
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);
@@ -128,44 +119,12 @@ public class RobotContainer {
// 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。
RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
// 左保险：按住刹车。
controllerlower.leftBumper()
.onTrue(Commands.runOnce(driveControls::emergencyStop))
.whileTrue(driveControls.brakeWhileHeld());
// 右保险：按住开启加速模式，松开后恢复普通速度。
controllerlower.rightBumper()
.onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
.onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
// Back 键：重置场地坐标系角度。
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
// Start 键：强行使用 shooter 侧 Limelight 的 MegaTag2 全场定位。
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
controllerlower.a().whileTrue(drivetrain.run(() -> {
drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
System.out.println("Try using shooter for metatag2");
}));
controllerlower.b().whileTrue(drivetrain.run(() -> {
drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
System.out.println("Try using intaker for metatag2");
}));
controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
// 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
// controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand());
// controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand());
// 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。
Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。
autoAim.onTrue(operatorControls.requestAimHubCommand());
autoAim.onFalse(operatorControls.requestIdleCommand());
// 上操作员左保险：按住进入传球模式，松开回到全手动模式。
Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。
autoAim2.onTrue(operatorControls.requestPassBallCommand());
autoAim2.onFalse(operatorControls.requestIdleCommand());
// controller.leftStick().onTrue(operatorControls.requestAimHubCommand());
// controller.leftStick().onFalse(operatorControls.requestIdleCommand());
}
@@ -175,18 +134,17 @@ public class RobotContainer {
// 这些命令 require Superstructure，与自动程序共用需求；
// 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
Trigger teleopOnly = RobotModeTriggers.teleop();
controllerupper.rightTrigger().and(teleopOnly)
controller.rightTrigger().and(teleopOnly)
.onTrue(operatorControls.requestShootHubCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.b().and(teleopOnly)
controller.rightBumper().and(teleopOnly)
.onTrue(operatorControls.requestEjectCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0)));
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0)));
controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets()));
controllerupper.start().onTrue(operatorControls.resetBackboardCommand());
controller.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0)));
controller.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0)));
controller.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
controller.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
controller.start().onTrue(operatorControls.resetBackboardCommand());
}
private void bingdingLED() {
@@ -198,16 +156,13 @@ public class RobotContainer {
}
public void tunerPeriodic() {
boolean allowMotorOutputTuning = DriverStation.isTest();
driveGainsTuner.periodic();
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
shooterSubsystem.debugFlywheelVelocityPeriodic();
configConveyorTuner.periodic(null);
shooterSubsystem.debugShooterConveyorVelocityPeriodic();
configBackboardTuner.periodic();
shooterSubsystem.debugBackboardTargetPeriodic();
configMainConveyorTuner.periodic(null);
configIntakerRollerTuner.periodic(null);
configIntakerRotaterTuner.periodic(null);
shooterSubsystem.debugFlywheelVelocityPeriodic(allowMotorOutputTuning);
shooterSubsystem.debugBackboardTargetPeriodic(allowMotorOutputTuning);
shooterSubsystem.setRightFlywheelFollowLeft();
shooterSubsystem.setRightConveyorFollowLeft();
}
@@ -219,27 +174,24 @@ public class RobotContainer {
private void configueIntaker() {
// 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。
Trigger teleopOnly = RobotModeTriggers.teleop();
controllerupper.leftTrigger().and(teleopOnly)
controller.leftTrigger().and(teleopOnly)
.onTrue(operatorControls.requestPassBallCommand())
.onFalse(operatorControls.requestIdleCommand());
controller.leftBumper().and(teleopOnly)
.onTrue(operatorControls.requestIntakeCommand())
.onFalse(operatorControls.requestIdleCommand());
Trigger manualRaiseIntaker = controllerupper.x().and(teleopOnly);
Trigger manualRaiseIntaker = controller.y().and(teleopOnly);
manualRaiseIntaker
.onTrue(operatorControls.requestManualCommand())
.whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem))
.onFalse(operatorControls.requestIdleCommand());
Trigger manualLowerIntaker = controllerupper.a().and(teleopOnly);
Trigger manualLowerIntaker = controller.a().and(teleopOnly);
manualLowerIntaker
.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/DriveControls.java
+3 -3
View File
@@ -37,8 +37,8 @@ public class DriveControls {
private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
// 默认速度比例，普通驾驶使用。
private double normalDriveScale = 0.3;
private double normalTurnScale = 0.7;
private double normalDriveScale = 1.0;
private double normalTurnScale = 1.0;
// 加速模式比例，按住 RB 时使用。
private double boostDriveScale = 1.0;
@@ -48,7 +48,7 @@ public class DriveControls {
private boolean boostEnabled = false;
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效加速度限制
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(3.5); // m/s^2 等效加速度限制
private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制
src/main/java/frc/robot/controls/OperatorControls.java
+19
View File
@@ -3,6 +3,7 @@ package frc.robot.controls;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants;
import frc.robot.subsystems.IntakeRollerSubsystem;
import frc.robot.subsystems.IntakeRotaterSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.Superstructure;
@@ -74,6 +75,24 @@ public class OperatorControls {
return Commands.runEnd(intakeRotater::manualRaise, intakeRotater::stop, intakeRotater);
}
public Command manualRaiseIntakeWithRollerCommand(
IntakeRotaterSubsystem intakeRotater,
IntakeRollerSubsystem intakeRoller
) {
return Commands.runEnd(
() -> {
intakeRotater.manualRaise();
intakeRoller.intake();
},
() -> {
intakeRotater.stop();
intakeRoller.stop();
},
intakeRotater,
intakeRoller
);
}
public Command manualLowerIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) {
return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater);
}
src/main/java/frc/robot/generated/TunerConstants.java
+1 -1
View File
@@ -77,7 +77,7 @@ public class TunerConstants {
new Pigeon2Configuration()
.withGyroTrim(
new GyroTrimConfigs()
.withGyroScalarZ(-5.0)
.withGyroScalarZ(-6.7)
)
.withPigeon2Features(
new Pigeon2FeaturesConfigs()
src/main/java/frc/robot/subsystems/LimelightSubsystem.java
-4
View File
@@ -46,8 +46,4 @@ public class LimelightSubsystem extends SubsystemBase {
public Pose2d getEstimatedPose() {
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
}
public double getShooterVelocityByDistance() {
return Constants.Shooter.flywheelVelocityByDistance.get(getDistanceToTarget());
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+27 -51
View File
@@ -9,12 +9,14 @@ import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
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.DriverStation;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
@@ -49,7 +51,6 @@ public class ShooterSubsystem extends SubsystemBase {
private final NetworkTable shooterNetworkTable;
private final NetworkTable flywheelTuningTable;
private final NetworkTable conveyorTuningTable;
private final NetworkTable backboardTuningTable;
private double flywheelOutputVolts = 0.0;
@@ -90,6 +91,7 @@ public class ShooterSubsystem extends SubsystemBase {
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
setBackboardBrakeMode();
backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
backboardEncoder.setSamplesToAverage(5);
@@ -109,9 +111,6 @@ public class ShooterSubsystem extends SubsystemBase {
flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false);
flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity);
flywheelTuningTable.getEntry("flywheelVelocityTolerance").setDefaultDouble(Constants.Shooter.flywheelReadyVelocityTolerance);
conveyorTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/conveyor");
conveyorTuningTable.getEntry("conveyorVelocityEnable").setDefaultBoolean(false);
conveyorTuningTable.getEntry("conveyorTargetVelocity").setDefaultDouble(Constants.Shooter.shooterConveyorFeedVelocity);
backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
@@ -155,18 +154,14 @@ public class ShooterSubsystem extends SubsystemBase {
}
public boolean atVelocity() {
if (Math.abs(flywheelTargetVelocity) < Constants.Shooter.flywheelReadyVelocityThreshold) {
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
double currentvelocity = getAverageFlywheelVelocity();
if(currentvelocity>flywheelTargetVelocity*0.95){
return true;
} else {
return false;
}
if (Math.abs(getFlywheelVelocityError()) > flywheelVelocityTolerance) {
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
return false;
}
return Timer.getFPGATimestamp() - flywheelVelocityReadyStartTimestamp
>= Constants.Shooter.flywheelReadyDelaySeconds;
}
}
public double getLeftFlywheelVelocity() {
return leftFlywheel.getVelocity().getValueAsDouble();
}
@@ -196,11 +191,7 @@ public class ShooterSubsystem extends SubsystemBase {
/* ====================== */
public void runShooterConveyorVelocity(double targetVelocityRps) {
shooterConveyorTargetVelocity = MathUtil.clamp(
targetVelocityRps,
-Constants.Shooter.shooterConveyorMaxVelocity,
Constants.Shooter.shooterConveyorMaxVelocity
);
shooterConveyorTargetVelocity = targetVelocityRps;
leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity));
setRightConveyorFollowLeft();
}
@@ -318,6 +309,12 @@ public class ShooterSubsystem extends SubsystemBase {
setBackboardVoltage(0.0);
}
public void setBackboardBrakeMode() {
var backboardMotorOutputConfigs = new MotorOutputConfigs();
backboardMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake;
backboardMotor.getConfigurator().apply(backboardMotorOutputConfigs);
}
public double getBackboardPosition() {
return backboardEncoder.getDistance();
}
@@ -339,17 +336,17 @@ public class ShooterSubsystem extends SubsystemBase {
}
public boolean isBackboardPositionTuningActive() {
return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
return DriverStation.isTest()
&& backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
}
public boolean isFlywheelTuningControlActive() {
boolean velocityTunerEnabled = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
return velocityTunerEnabled;
return DriverStation.isTest()
&& flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
}
public boolean isShooterConveyorTuningControlActive() {
boolean velocityTunerEnabled = conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
return velocityTunerEnabled;
return false;
}
public void resetbackboardencoder() {
@@ -379,8 +376,9 @@ public class ShooterSubsystem extends SubsystemBase {
stopBackboard();
}
public void debugBackboardTargetPeriodic() {
boolean positionEnable = backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
public void debugBackboardTargetPeriodic(boolean allowMotorOutput) {
boolean positionEnable =
allowMotorOutput && backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0);
double clampedTarget = MathUtil.clamp(
requestedTarget,
@@ -397,8 +395,9 @@ public class ShooterSubsystem extends SubsystemBase {
}
}
public void debugFlywheelVelocityPeriodic() {
flywheelVelocityTuningActive = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
public void debugFlywheelVelocityPeriodic(boolean allowMotorOutput) {
flywheelVelocityTuningActive =
allowMotorOutput && flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
double requestedTargetVelocity =
flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity);
flywheelVelocityTolerance = Math.max(
@@ -422,28 +421,6 @@ public class ShooterSubsystem extends SubsystemBase {
}
}
public void debugShooterConveyorVelocityPeriodic() {
shooterConveyorVelocityTuningActive =
conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
double requestedTargetVelocity =
conveyorTuningTable.getEntry("conveyorTargetVelocity")
.getDouble(Constants.Shooter.shooterConveyorFeedVelocity);
double clampedTargetVelocity = MathUtil.clamp(
requestedTargetVelocity,
-Constants.Shooter.shooterConveyorMaxVelocity,
Constants.Shooter.shooterConveyorMaxVelocity
);
conveyorTuningTable.getEntry("conveyorVelocityEnableActive")
.setBoolean(shooterConveyorVelocityTuningActive);
conveyorTuningTable.getEntry("conveyorCommandedTargetVelocity").setDouble(requestedTargetVelocity);
conveyorTuningTable.getEntry("conveyorClampedTargetVelocity").setDouble(clampedTargetVelocity);
if (shooterConveyorVelocityTuningActive) {
runShooterConveyorVelocity(clampedTargetVelocity);
}
}
@Override
public void periodic() {
double backboardPosition = getBackboardPosition();
@@ -499,4 +476,3 @@ public class ShooterSubsystem extends SubsystemBase {
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
}
}
src/main/java/frc/robot/subsystems/Superstructure.java
+12 -4
View File
@@ -1,5 +1,6 @@
package frc.robot.subsystems;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
@@ -284,8 +285,13 @@ public class Superstructure extends SubsystemBase {
}
private void handleShootHub() {
double targetVelocity = limelight.getShooterVelocityByDistance() + flywheelVelocityOffset;
double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
double x = NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
x = MathUtil.clamp(x, 1.2, 4.5);
double rawtargetVelocity = -0.9702766369910669*x*x*x*x*x*x+16.17984114722666*x*x*x*x*x-107.89020547785285*x*x*x*x+365.6895126610877*x*x*x-659.1553030522557*x*x+598.8266122952737*x-188.96203796840197;
double targetVelocity = rawtargetVelocity + flywheelVelocityOffset;
double rawtargetbackboardPosition = +72.63723930800967*x*x*x*x*x*x-1226.7072434356646*x*x*x*x*x+8380.732539436454*x*x*x*x-29585.06108203164*x*x*x+56753.91414329895*x*x-55469.56099616276*x+21418.08191862802;
rawtargetbackboardPosition = MathUtil.clamp(rawtargetbackboardPosition, 0, 2200);
double backboardPosition = rawtargetbackboardPosition + backboardPositionOffset;
handlePreparedFeed(targetVelocity, backboardPosition);
}
@@ -301,8 +307,6 @@ public class Superstructure extends SubsystemBase {
setSystemState(SystemState.PREP_SHOOT);
}
intakeRoller.stop();
conveyor.stop();
stopShooterConveyorUnlessTuning();
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
// 但不会绕过 Superstructure 状态机。
shooter.setFlywheelVelocity(targetVelocity);
@@ -312,6 +316,7 @@ public class Superstructure extends SubsystemBase {
// 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
// 避免球接触飞轮导致的掉速中断喂球。
intakeRotater.shootAssist();
conveyor.feedToShooter();
runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity);
return;
@@ -382,6 +387,9 @@ public class Superstructure extends SubsystemBase {
if (systemState != nextState) {
systemState = nextState;
stateStartTimestamp = Timer.getFPGATimestamp();
if (nextState == SystemState.SHOOTING) {
shooter.setBackboardBrakeMode();
}
}
}
src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java
+1 -18
View File
@@ -1,6 +1,5 @@
package frc.robot.tuning;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFXS;
import edu.wpi.first.networktables.*;
@@ -20,11 +19,8 @@ public class ConfigTalonFXSMotorTuner {
private final DoubleSubscriber KaSub;
private final DoubleSubscriber KsSub;
private final BooleanSubscriber enableRuningSub;
private final DoubleSubscriber runningSpeedRPS;
private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
public ConfigTalonFXSMotorTuner(TalonFXS motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
this.motor = motor_;
this.motorName = motorName_;
@@ -39,9 +35,6 @@ public class ConfigTalonFXSMotorTuner {
table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA);
table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);
table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);
this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI);
@@ -49,8 +42,6 @@ public class ConfigTalonFXSMotorTuner {
this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV);
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);
}
public void periodic() {
@@ -73,13 +64,5 @@ public class ConfigTalonFXSMotorTuner {
lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks;
}
if (enableRuningSub.get()) {;
double speed = runningSpeedRPS.get();
motor.setControl(velocityRequest.withVelocity(speed));
// System.out.println("running motor with speed: "+speed+" on motor: "+motorName);
}else{
motor.setControl(velocityRequest.withVelocity(0));
}
}
}