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 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)
src/main/java/frc/robot/RobotContainer.java
+4 -6
View File
@@ -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)));
}
/* ====================== */
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 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);
@@ -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 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 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;
// }
public void adjustFlywheelSpeedOffset(double 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());
}
private double getDistance(){
public double getDistance(){
//获得车子距离Hub的距离
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
}