From b347e930781d1718e3070558c2accfa112a1b199 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Mon, 23 Feb 2026 18:04:01 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=93=E8=A1=A8=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 | 2 +- src/main/java/frc/robot/RobotContainer.java | 10 +- .../frc/robot/controls/ShooterControls.java | 136 +++++++++--------- .../robot/subsystems/ShooterSubsystem.java | 2 +- 4 files changed, 76 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2615536..5a16862 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,7 +9,7 @@ public final class Constants { public static final double conveyorSpeed = 20; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) - .withKV(0.1).withKA(0.0).withKS(0.15); + .withKV(0.0088).withKA(0.0).withKS(0.0); public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 79fb7b0..e663b52 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,12 +102,10 @@ public class RobotContainer { private void configueShooter(){ shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand()); - controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-5))); - controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(5))); - controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(500))); - controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-500))); - - controller.b().onTrue(shooterControls.resetBackboardCommand()); + controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1))); + controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1))); + controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100))); + controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100))); } /* ====================== */ diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index c92fd2f..df4907e 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.ShooterSubsystem.Mode; public class ShooterControls { private final ShooterSubsystem shooterSubsystem; @@ -20,7 +19,7 @@ public class ShooterControls { this.shooterSubsystem = shooterSubsystem; this.controller = controller; - shooterControlTable = NetworkTableInstance.getDefault().getTable("shooterControl"); + shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl"); } public Command defaultShooterCommand() { @@ -28,8 +27,12 @@ public class ShooterControls { double flywheelSpeed; double backboardPosition; double conveyorSpeed; - flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(30); - backboardPosition = calculateBackboardPositionOnlywithOffset(100); + // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60); + var x = shooterSubsystem.getDistance(); + flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351; + flywheelSpeed += flywheelSpeedOffset; + backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684; + backboardPosition += backboardPositionOffset; conveyorSpeed = Constants.Shooter.conveyorSpeed; shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed); shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed); @@ -39,7 +42,7 @@ public class ShooterControls { flywheelSpeed = 0; conveyorSpeed = 0; } - + // double flywheelSpeed = calculateFlywheelSpeed(0); // double BackboardPosition = calculateBackboardPosition(0); shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed); @@ -79,70 +82,71 @@ public class ShooterControls { } //打表参数 - private static final double[][] FLYWHEEL_TABLE = { - {1.5, 0.42}, - {2.0, 0.46}, - {2.5, 0.51}, - {3.0, 0.57}, - {3.5, 0.64}, - {4.0, 0.72} - }; + // private static final double[][] FLYWHEEL_TABLE = { + // {1.5, 0.42}, + // {2.0, 0.46}, + // {2.5, 0.51}, + // {3.0, 0.57}, + // {3.5, 0.64}, + // {4.0, 0.72} + // }; - //线性插值函数 - private static double lerp(double x0, double y0, double x1, double y1, double x) { - if (Math.abs(x1 - x0) < 1e-9) { - return y0; - } - double t = (x - x0) / (x1 - x0); - return y0 + t * (y1 - y0); - } - private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) { - return defaultSpeed + flywheelSpeedOffset; - } - private double calculateBackboardPositionOnlywithOffset(double defaultPosition) { - return defaultPosition + backboardPositionOffset; - } + // //线性插值函数 + // private static double lerp(double x0, double y0, double x1, double y1, double x) { + // if (Math.abs(x1 - x0) < 1e-9) { + // return y0; + // } + // double t = (x - x0) / (x1 - x0); + // return y0 + t * (y1 - y0); + // } + // private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) { + // return defaultSpeed + flywheelSpeedOffset; + // } + // private double calculateBackboardPositionOnlywithOffset(double defaultPosition) { + // return defaultPosition + backboardPositionOffset; + // } - private double calculateFlywheelSpeed(double distance) { - // 小于最小距离：用第一个点 - if (distance <= FLYWHEEL_TABLE[0][0]) { - return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset; - } - // 大于最大距离：用最后一个点 - int last = FLYWHEEL_TABLE.length - 1; - if (distance >= FLYWHEEL_TABLE[last][0]) { - return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset; - } - // 查找区间并插值 - for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) { - double d0 = FLYWHEEL_TABLE[i][0]; - double s0 = FLYWHEEL_TABLE[i][1]; - double d1 = FLYWHEEL_TABLE[i + 1][0]; - double s1 = FLYWHEEL_TABLE[i + 1][1]; - if (distance >= d0 && distance <= d1) { - return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset; - } - } - return 0; - } + + // private double calculateFlywheelSpeed(double distance) { + // // 小于最小距离：用第一个点 + // if (distance <= FLYWHEEL_TABLE[0][0]) { + // return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset; + // } + // // 大于最大距离：用最后一个点 + // int last = FLYWHEEL_TABLE.length - 1; + // if (distance >= FLYWHEEL_TABLE[last][0]) { + // return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset; + // } + // // 查找区间并插值 + // for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) { + // double d0 = FLYWHEEL_TABLE[i][0]; + // double s0 = FLYWHEEL_TABLE[i][1]; + // double d1 = FLYWHEEL_TABLE[i + 1][0]; + // double s1 = FLYWHEEL_TABLE[i + 1][1]; + // if (distance >= d0 && distance <= d1) { + // return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset; + // } + // } + // return 0; + // } - private double calculateBackboardPosition(double distance) { - double BackboardPosition; - // 距离阈值（单位自己按distance进行修改） - final double DISTANCE_NEAR = 2.0; // 近距离阈值 - final double DISTANCE_MID = 4.0; // 中距离阈值 - if (distance < DISTANCE_NEAR) { - BackboardPosition = Constants.ShooterCalculation.positionNear; - } - else if (distance < DISTANCE_MID) { - BackboardPosition = Constants.ShooterCalculation.positionMid; - } - else { - BackboardPosition = Constants.ShooterCalculation.postionFar; - } - BackboardPosition += backboardPositionOffset; - return BackboardPosition; - } + // private double calculateBackboardPosition(double distance) { + // double BackboardPosition; + // // 距离阈值（单位自己按distance进行修改） + // final double DISTANCE_NEAR = 2.0; // 近距离阈值 + // final double DISTANCE_MID = 4.0; // 中距离阈值 + // if (distance < DISTANCE_NEAR) { + // BackboardPosition = Constants.ShooterCalculation.positionNear; + // } + // else if (distance < DISTANCE_MID) { + // BackboardPosition = Constants.ShooterCalculation.positionMid; + // } + // else { + // BackboardPosition = Constants.ShooterCalculation.postionFar; + // } + // BackboardPosition += backboardPositionOffset; + // return BackboardPosition; + // } public void adjustFlywheelSpeedOffset(double offset) { flywheelSpeedOffset += offset; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 170789d..626d3bc 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -164,7 +164,7 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name()); } - private double getDistance(){ + public double getDistance(){ //获得车子距离Hub的距离 return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0); }