From 914584b71c93a36e0079f7e7564bed4b789153c0 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Thu, 9 Jul 2026 11:58:36 +0800 Subject: [PATCH] =?UTF-8?q?=E8=83=8C=E6=9D=BF=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 9 ++- .../robot/subsystems/ShooterSubsystem.java | 60 ++++++++++++++----- .../frc/robot/subsystems/Superstructure.java | 14 +++-- 3 files changed, 63 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 86c2f8d..542ae43 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,8 +53,13 @@ public final class Constants { public static final double kD = 0.0; } - public static final double backboardSpeedMax = 5.0; - public static final double backboardSpeedMin = 0.4; + public static final double backboardSpeedMax = 3.0; + public static final double backboardSpeedMin = 0; + public static final double backboardGravityFeedforwardMinVolts = 0.1; + public static final double backboardGravityFeedforwardMaxVolts = 0.3; + public static final double backboardGravityFeedforwardMinPosition = 0.0; + public static final double backboardGravityFeedforwardMaxPosition = 2200.0; + public static final double backboardGravityFeedforwardDirection = 1.0; public static final double backboardUpLimit = 2200.0; public static final double backboardDownLimit = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2495d54..efa0772 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -54,6 +54,7 @@ public class ShooterSubsystem extends SubsystemBase { private double shooterConveyorOutputVolts = 0.0; private double backboardOutputVolts = 0.0; private double backboardRawPidOutputVolts = 0.0; + private double backboardGravityFeedforwardVolts = 0.0; private double backboardClampedPidOutputVolts = 0.0; private boolean backboardOutputSaturated = false; private boolean backboardMinimumOutputActive = false; @@ -203,7 +204,8 @@ public class ShooterSubsystem extends SubsystemBase { public void outputBackboard() { // 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。 - double rawPidOutput = backboardPID.calculate(getBackboardPosition()); + double backboardPosition = getBackboardPosition(); + double rawPidOutput = backboardPID.calculate(backboardPosition); double pidOutput = MathUtil.clamp( rawPidOutput, -Constants.Shooter.backboardSpeedMax, @@ -220,26 +222,50 @@ public class ShooterSubsystem extends SubsystemBase { backboardMinimumOutputActive = false; } - shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput); - setBackboardVoltage(pidOutput); + double gravityOutput = calculateBackboardGravityFeedforward(backboardPosition); + double outputVolts = MathUtil.clamp( + pidOutput + gravityOutput, + -Constants.Shooter.backboardSpeedMax, + Constants.Shooter.backboardSpeedMax + ); + backboardGravityFeedforwardVolts = gravityOutput; + backboardOutputSaturated = backboardOutputSaturated + || Math.abs((pidOutput + gravityOutput) - outputVolts) > 1e-9; + + shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(outputVolts); + setBackboardVoltage(outputVolts); + } + + private double calculateBackboardGravityFeedforward(double backboardPosition) { + double clampedPosition = MathUtil.clamp( + backboardPosition, + Constants.Shooter.backboardGravityFeedforwardMinPosition, + Constants.Shooter.backboardGravityFeedforwardMaxPosition + ); + double positionRange = Constants.Shooter.backboardGravityFeedforwardMaxPosition + - Constants.Shooter.backboardGravityFeedforwardMinPosition; + if (positionRange <= 0.0) { + return 0.0; + } + + double positionRatio = (clampedPosition - Constants.Shooter.backboardGravityFeedforwardMinPosition) + / positionRange; + double feedforwardVolts = Constants.Shooter.backboardGravityFeedforwardMinVolts + + positionRatio + * (Constants.Shooter.backboardGravityFeedforwardMaxVolts + - Constants.Shooter.backboardGravityFeedforwardMinVolts); + return Constants.Shooter.backboardGravityFeedforwardDirection * feedforwardVolts; } public void holdBackboardAt(double targetPosition) { - // 上层只给目标位置；到位后停止输出，未到位时继续闭环。 + // 上层只给目标位置；到位后仍继续输出重力补偿。 setBackboardPosition(targetPosition); - if (isBackboardAtTarget()) { - backboardRawPidOutputVolts = 0.0; - backboardClampedPidOutputVolts = 0.0; - backboardOutputSaturated = false; - backboardMinimumOutputActive = false; - setBackboardVoltage(0.0); - } else { - outputBackboard(); - } + outputBackboard(); } public void stopBackboard() { backboardRawPidOutputVolts = 0.0; + backboardGravityFeedforwardVolts = 0.0; backboardClampedPidOutputVolts = 0.0; backboardOutputSaturated = false; backboardMinimumOutputActive = false; @@ -254,6 +280,10 @@ public class ShooterSubsystem extends SubsystemBase { return backboardPID.atGoal(); } + public boolean isBackboardPositionTuningActive() { + return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); + } + public void resetbackboardencoder() { backboardEncoder.reset(); backboardPID.reset(0); @@ -301,7 +331,6 @@ public class ShooterSubsystem extends SubsystemBase { @Override public void periodic() { - backboardPID.calculate(getBackboardPosition()); double backboardPosition = getBackboardPosition(); double backboardGoalPosition = backboardPID.getGoal().position; double backboardSetpointPosition = backboardPID.getSetpoint().position; @@ -324,11 +353,14 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("backboardGoalError").setDouble(backboardGoalPosition - backboardPosition); shooterNetworkTable.getEntry("backboardSetpointError").setDouble(backboardSetpointPosition - backboardPosition); shooterNetworkTable.getEntry("backboardRawPIDOutput").setDouble(backboardRawPidOutputVolts); + shooterNetworkTable.getEntry("backboardGravityFeedforward").setDouble(backboardGravityFeedforwardVolts); 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("backboardGravityFeedforwardMinVolts").setDouble(Constants.Shooter.backboardGravityFeedforwardMinVolts); + shooterNetworkTable.getEntry("backboardGravityFeedforwardMaxVolts").setDouble(Constants.Shooter.backboardGravityFeedforwardMaxVolts); shooterNetworkTable.getEntry("backboardPositionTolerance").setDouble(10.0); shooterNetworkTable.getEntry("backboardShootPosition").setDouble(Constants.Superstructure.backboardShootPosition); shooterNetworkTable.getEntry("backboardPassPosition").setDouble(Constants.Superstructure.passBallBackboardPosition); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 966141e..ff64917 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -230,14 +230,14 @@ public class Superstructure extends SubsystemBase { intakeRotater.stop(); shooter.stopFlywheel(); shooter.stopShooterConveyor(); - shooter.holdBackboardAt(0.0); + holdBackboardStowedUnlessTuning(); setSystemState(SystemState.STOWED); } private void handleIntake() { shooter.stopFlywheel(); shooter.stopShooterConveyor(); - shooter.holdBackboardAt(0.0); + holdBackboardStowedUnlessTuning(); // Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。 intakeRoller.intake(); conveyor.feedToShooter(); @@ -249,7 +249,7 @@ public class Superstructure extends SubsystemBase { conveyor.stop(); shooter.stopFlywheel(); shooter.stopShooterConveyor(); - shooter.holdBackboardAt(0.0); + holdBackboardStowedUnlessTuning(); setSystemState(SystemState.STOWED); } @@ -309,7 +309,13 @@ public class Superstructure extends SubsystemBase { conveyor.reverse(); shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage); shooter.stopFlywheel(); - shooter.holdBackboardAt(0.0); + holdBackboardStowedUnlessTuning(); + } + + private void holdBackboardStowedUnlessTuning() { + if (!shooter.isBackboardPositionTuningActive()) { + shooter.holdBackboardAt(0.0); + } } private void handleManual() {