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

打表完成

Browse Source
This commit is contained in:
OnDYCLaptop
2026-02-23 18:04:01 +08:00
parent da6e3d4a5a
commit b347e93078
4 changed files with 76 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/Constants.java
+1 -1
View File
@@ -9,7 +9,7 @@ public final class Constants {
public static final double conveyorSpeed = 20; public static final double conveyorSpeed = 20;
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0) .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() public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0) .withKP(0.0).withKI(0).withKD(0)
src/main/java/frc/robot/RobotContainer.java
+4 -6
View File
@@ -102,12 +102,10 @@ public class RobotContainer {
private void configueShooter(){ private void configueShooter(){
shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand()); shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-5))); controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(5))); controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(500))); controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-500))); controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
controller.b().onTrue(shooterControls.resetBackboardCommand());
} }
/* ====================== */ /* ====================== */
src/main/java/frc/robot/controls/ShooterControls.java
+69 -65
View File
@@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants; import frc.robot.Constants;
import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.ShooterSubsystem.Mode;
public class ShooterControls { public class ShooterControls {
private final ShooterSubsystem shooterSubsystem; private final ShooterSubsystem shooterSubsystem;
@@ -20,7 +19,7 @@ public class ShooterControls {
this.shooterSubsystem = shooterSubsystem; this.shooterSubsystem = shooterSubsystem;
this.controller = controller; this.controller = controller;
shooterControlTable = NetworkTableInstance.getDefault().getTable("shooterControl"); shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl");
} }
public Command defaultShooterCommand() { public Command defaultShooterCommand() {
@@ -28,8 +27,12 @@ public class ShooterControls {
double flywheelSpeed; double flywheelSpeed;
double backboardPosition; double backboardPosition;
double conveyorSpeed; double conveyorSpeed;
flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(30); // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
backboardPosition = calculateBackboardPositionOnlywithOffset(100); 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; conveyorSpeed = Constants.Shooter.conveyorSpeed;
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed); shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed); shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
@@ -79,70 +82,71 @@ public class ShooterControls {
} }
//打表参数 //打表参数
private static final double[][] FLYWHEEL_TABLE = { // private static final double[][] FLYWHEEL_TABLE = {
{1.5, 0.42}, // {1.5, 0.42},
{2.0, 0.46}, // {2.0, 0.46},
{2.5, 0.51}, // {2.5, 0.51},
{3.0, 0.57}, // {3.0, 0.57},
{3.5, 0.64}, // {3.5, 0.64},
{4.0, 0.72} // {4.0, 0.72}
}; // };
//线性插值函数 // //线性插值函数
private static double lerp(double x0, double y0, double x1, double y1, double x) { // private static double lerp(double x0, double y0, double x1, double y1, double x) {
if (Math.abs(x1 - x0) < 1e-9) { // if (Math.abs(x1 - x0) < 1e-9) {
return y0; // return y0;
} // }
double t = (x - x0) / (x1 - x0); // double t = (x - x0) / (x1 - x0);
return y0 + t * (y1 - y0); // return y0 + t * (y1 - y0);
} // }
private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) { // private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) {
return defaultSpeed + flywheelSpeedOffset; // return defaultSpeed + flywheelSpeedOffset;
} // }
private double calculateBackboardPositionOnlywithOffset(double defaultPosition) { // private double calculateBackboardPositionOnlywithOffset(double defaultPosition) {
return defaultPosition + backboardPositionOffset; // 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 calculateBackboardPosition(double distance) { // private double calculateFlywheelSpeed(double distance) {
double BackboardPosition; // // 小于最小距离：用第一个点
// 距离阈值（单位自己按distance进行修改） // if (distance <= FLYWHEEL_TABLE[0][0]) {
final double DISTANCE_NEAR = 2.0; // 近距离阈值 // return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset;
final double DISTANCE_MID = 4.0; // 中距离阈值 // }
if (distance < DISTANCE_NEAR) { // // 大于最大距离：用最后一个点
BackboardPosition = Constants.ShooterCalculation.positionNear; // int last = FLYWHEEL_TABLE.length - 1;
} // if (distance >= FLYWHEEL_TABLE[last][0]) {
else if (distance < DISTANCE_MID) { // return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset;
BackboardPosition = Constants.ShooterCalculation.positionMid; // }
} // // 查找区间并插值
else { // for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) {
BackboardPosition = Constants.ShooterCalculation.postionFar; // double d0 = FLYWHEEL_TABLE[i][0];
} // double s0 = FLYWHEEL_TABLE[i][1];
BackboardPosition += backboardPositionOffset; // double d1 = FLYWHEEL_TABLE[i + 1][0];
return BackboardPosition; // 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;
// }
public void adjustFlywheelSpeedOffset(double offset) { public void adjustFlywheelSpeedOffset(double offset) {
flywheelSpeedOffset += offset; flywheelSpeedOffset += offset;
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+1 -1
View File
@@ -164,7 +164,7 @@ public class ShooterSubsystem extends SubsystemBase {
shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name()); shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name());
} }
private double getDistance(){ public double getDistance(){
//获得车子距离Hub的距离 //获得车子距离Hub的距离
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0); return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
} }