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

shoot逻辑修复 新老状态机合并

Browse Source
This commit is contained in:
OnDYCLaptop
2026-07-05 22:52:42 +08:00
parent a1fc79659e
commit 5947fa27d3
20 changed files with 193 additions and 282 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto
+2 -2
View File
@@ -13,13 +13,13 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
}
]
src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto
+1 -1
View File
@@ -13,7 +13,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
}
]
src/main/deploy/pathplanner/autos/Copy of Shoot and Depot Blue.auto
+3 -3
View File
@@ -30,7 +30,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
@@ -100,7 +100,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
@@ -125,7 +125,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
src/main/deploy/pathplanner/autos/Depot Auto Blue.auto
+1 -1
View File
@@ -76,7 +76,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
}
]
src/main/deploy/pathplanner/autos/Depot Auto Red.auto
+1 -1
View File
@@ -76,7 +76,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
}
]
src/main/deploy/pathplanner/autos/New Auto.auto
+1 -1
View File
@@ -19,7 +19,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
}
]
src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto
+1 -1
View File
@@ -13,7 +13,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto
+1 -1
View File
@@ -26,7 +26,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto
+3 -3
View File
@@ -30,7 +30,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
@@ -100,7 +100,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
@@ -125,7 +125,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto
+3 -3
View File
@@ -26,7 +26,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
@@ -99,7 +99,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
@@ -124,7 +124,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto
+2 -2
View File
@@ -32,7 +32,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
@@ -93,7 +93,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto
+2 -2
View File
@@ -32,7 +32,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
@@ -93,7 +93,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto
+2 -2
View File
@@ -32,7 +32,7 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "shoot_hub"
}
},
{
@@ -93,7 +93,7 @@
{
"type": "named",
"data": {
"name": "AutoShoot"
"name": "shoot_hub"
}
},
{
src/main/java/frc/robot/Constants.java
+2 -10
View File
@@ -10,15 +10,6 @@ public final class Constants {
public static final boolean LEDUsing = false;
public enum RobotStatus {
Stopped,
AllTelop,
PassingBall,
AutoAimming,
CrossingBump,
CrossingTrench
}
public static class Shooter {
// 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。
public static final int leftFlywheelID = 41;
@@ -166,10 +157,11 @@ public final class Constants {
// 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
public static final double shooterFeedPercent = 1.0;
public static final double shooterReversePercent = -0.6;
public static final double cleanupSeconds = 0.3;
public static final double shootTimeoutSeconds = 1.0;
public static final double aimToleranceDegrees = 1.5;
public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
public static final double passBallFlywheelRps = Shooter.defaultFlywheelTargetRps;
public static final double passBallBackboardPosition = 175.0;
// 射击时背板的目标位置（外接编码器计数）。
public static final double backboardShootPosition = 175.0;
src/main/java/frc/robot/RobotContainer.java
+8 -21
View File
@@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.RobotStatus;
import frc.robot.controls.Autocommand;
import frc.robot.controls.DriveControls;
import frc.robot.controls.OperatorControls;
@@ -67,13 +66,11 @@ public class RobotContainer {
// LED 子系统仍然只负责灯带显示，不参与机构状态判断。
private final LEDSubsystem leds = new LEDSubsystem(0, 72, 0); // 总长度 = 72 + 72
private final RobotStatusManager robotStatusManager = new RobotStatusManager();
/* ====================== */
/* 控制封装/调参 */
/* ====================== */
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager);
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, superstructure);
// 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
@@ -151,26 +148,19 @@ public class RobotContainer {
}));
// 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
// controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
// controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
// controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand());
// controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand());
// 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。
Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。
autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
autoAim.onTrue(operatorControls.requestAimHubCommand());
autoAim.onFalse(operatorControls.requestIdleCommand());
// 上操作员左保险：按住进入传球模式，松开回到全手动模式。
Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。
autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
autoAim2.onTrue(operatorControls.requestPassBallCommand());
autoAim2.onFalse(operatorControls.requestIdleCommand());
controllerlower.leftTrigger()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
controllerlower.rightTrigger()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
}
private void configueShooter() {
@@ -180,10 +170,7 @@ public class RobotContainer {
// 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
Trigger teleopOnly = RobotModeTriggers.teleop();
controllerupper.rightTrigger().and(teleopOnly)
.onTrue(operatorControls.requestShootCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.y().and(teleopOnly)
.onTrue(operatorControls.requestPrepShootCommand())
.onTrue(operatorControls.requestShootHubCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.b().and(teleopOnly)
.onTrue(operatorControls.requestEjectCommand())
src/main/java/frc/robot/RobotStatusManager.java
-25
View File
@@ -1,25 +0,0 @@
package frc.robot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants.RobotStatus;
public class RobotStatusManager {
private RobotStatus robotStatus;
public RobotStatusManager(){
this.robotStatus = RobotStatus.AllTelop;
}
public RobotStatus getStatus(){
return this.robotStatus;
}
public void setStatus(RobotStatus rs){
this.robotStatus = rs;
}
public Command setStatusCommand(RobotStatus rs){
return Commands.runOnce(()->setStatus(rs));
}
}
src/main/java/frc/robot/controls/Autocommand.java
+5 -7
View File
@@ -5,6 +5,7 @@ import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Constants;
import frc.robot.subsystems.IntakeRotaterSubsystem;
import frc.robot.subsystems.Superstructure;
@@ -28,10 +29,9 @@ public class Autocommand extends SequentialCommandGroup {
NamedCommands.registerCommand("stop_intake", idle(superstructure));
NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater));
NamedCommands.registerCommand("intaker_up", idle(superstructure));
NamedCommands.registerCommand("shoot", shoot(superstructure));
NamedCommands.registerCommand("shoot_hub", shootHub(superstructure));
NamedCommands.registerCommand("Shake", shake(driveControls));
NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
NamedCommands.registerCommand("AutoShoot", operatorControls.autoShootToHubCommand());
NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command());
}
@@ -56,12 +56,10 @@ public class Autocommand extends SequentialCommandGroup {
}
// 自动射击和手动射击使用同一套 gated shooting 流程。
public static Command shoot(Superstructure superstructure) {
public static Command shootHub(Superstructure superstructure) {
return Commands.sequence(
superstructure.requestPrepShootCommand(),
Commands.waitUntil(superstructure::canShoot).withTimeout(1.5),
superstructure.requestShootCommand(),
Commands.waitSeconds(1.2),
superstructure.requestShootHubCommand(),
Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
superstructure.requestIdleCommand()
);
}
src/main/java/frc/robot/controls/DriveControls.java
+75 -144
View File
@@ -15,11 +15,11 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.RobotStatusManager;
import frc.robot.Constants.DriveMode;
import frc.robot.Constants.RobotStatus;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Superstructure;
import frc.robot.subsystems.Superstructure.WantedState;
import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
@@ -36,35 +36,35 @@ public class DriveControls {
private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
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;
// 加速模式比例（按住 RB）
// 鍔犻€熸ā寮忔瘮渚嬶紙鎸変綇 RB锛?
private double boostDriveScale = 1.0;
private double boostTurnScale = 1.0;
//autoaim模式
//autoaim妯″紡
private double slowDriveScale =0.1;
private boolean boostEnabled = false;
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效（调）
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 绛夋晥锛堣皟锛?
private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调）
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 绛夋晥锛堣皟锛?
// ====== 类成员变量里新增：记录上一次输出 ======
// ====== 绫绘垚鍛樺彉閲忛噷鏂板锛氳褰曚笂涓€娆¤緭鍑?======
private double lastVx = 0.0;
private double lastVy = 0.0;
private double lastOmega = 0.0;
private NetworkTable autoControlNetworkTable;
private final RobotStatusManager robotStatusManager;
private final Superstructure superstructure;
public void setBoostEnabled(boolean enabled) {
boostEnabled = enabled;
}
// 新增：当前是否使用场地坐标系（默认 true）
// 鏂板锛氬綋鍓嶆槸鍚︿娇鐢ㄥ満鍦板潗鏍囩郴锛堥粯璁?true锛?
private boolean fieldCentricEnabled = true;
private final SwerveRequest.FieldCentric fieldCentric =
@@ -86,10 +86,10 @@ public class DriveControls {
.withRotationalDeadband(maxAngularRate * 0.05)
.withDriveRequestType(DriveRequestType.Velocity);
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, Superstructure superstructure) {
this.drivetrain = drivetrain;
this.driver = driver;
this.robotStatusManager = rsm;
this.superstructure = superstructure;
autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
}
@@ -101,16 +101,16 @@ public class DriveControls {
return fieldCentricEnabled;
}
/** 底盘的默认驾驶命令（teleop 期间持续执行） */
/** 搴曣洏鐨勯粯璁ら┚椹跺懡浠わ紙teleop 鏈熼棿鎸佺画鎵ц锛?*/
public Command defaultDriveCommand() {
return drivetrain.applyRequest(() -> {
SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
WantedState wantedState = superstructure.getWantedState();
SmartDashboard.putString("wantedState", wantedState.name());
double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
double turnScale = boostEnabled ? boostTurnScale : normalTurnScale;
double vx = -driver.getLeftY() * maxSpeed * driveScale;
double vy = -driver.getLeftX() * maxSpeed * driveScale;
double vomega = -driver.getRightX() * maxAngularRate * turnScale;
// ====== execute 内替换 limiter：只限加速、不限减速 ======
double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter);
@@ -123,105 +123,40 @@ public class DriveControls {
vy = limitedVy;
vomega = limitedOmega;
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
// 将距离值写入 NetworkTable
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
// 把朝向 hub 的目标角同步给底盘，供 Superstructure 读取 isAimed() 做射击门控；
// 数据来源与 AutoAimming 自动旋转完全相同，保证门控和实际瞄准一致。
drivetrain.setTargetHeading(
drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
double[] hubAim = calculateDistanceAndRotationToHub();
double[] passAim = calculateDistanceAndRotationToPassBall();
autoControlNetworkTable.getEntry("distanceToHub").setDouble(hubAim[0]);
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(hubAim[1]);
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(passAim[0]);
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(passAim[1]);
switch (robotStatusManager.getStatus()) {
case Stopped:
vx = 0;
vy = 0;
vomega = 0;
break;
case PassingBall:
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
case AutoAimming:
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
double[] activeAim = wantedState == WantedState.PASS_BALL ? passAim : hubAim;
drivetrain.setTargetHeading(
drivetrain.getRotation().plus(Rotation2d.fromDegrees(activeAim[1])));
switch (wantedState) {
case SHOOT_HUB:
case AIM_HUB:
vomega = calculateRotationSpeedFromRotationAngle(hubAim[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
vy = -driver.getLeftX() * maxSpeed * slowDriveScale;
break;
case AllTelop:
case PASS_BALL:
vomega = calculateRotationSpeedFromRotationAngle(passAim[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
default:
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
break;
case CrossingBump:
double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
if (Math.abs(differenceBump) < 0.1) {
vy = 0;
} else {
double raw = differenceBump * Constants.AutoPositioning.autoPositioningkP;
vy = Math.copySign(
Math.max(Math.abs(raw), 0.4),
raw
);
vy = isRedAlliance ? -vy : vy;
}
autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
Rotation2d BumpcurrentAngle = drivetrain.getState().Pose.getRotation();
double BumpcurrentRad = BumpcurrentAngle.getRadians();
double targetA = Math.toRadians(45);
double targetB = Math.toRadians(-45);
// 计算误差
double errorA = targetA - BumpcurrentRad;
double errorB = targetB - BumpcurrentRad;
// 角度归一化到 [-pi, pi]
errorA = Math.atan2(Math.sin(errorA), Math.cos(errorA));
errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
// 选误差绝对值更小的那个
double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
vomega = calculatevomegaFromRotationAngle(chosenError);
autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega);
//vomega = calculateRotationSpeedForBump();
break;
case CrossingTrench:
double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]);
if (Math.abs(differenceTrench) < 0.1) {
vy = 0;
} else {
double raw = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
vy = Math.copySign(
Math.max(Math.abs(raw), 0.4),
raw
);
vy = isRedAlliance ? -vy : vy;
}
autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
double currentRad = currentAngle.getRadians();
double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
double TrenchAngleerror = targetRad - currentRad;
vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega);
//vomega = calculateRotationSpeedForTrench();
break;
}
}
autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
boolean isAutoLike =
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
if (fieldCentricEnabled || isAutoLike) {
// 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
//var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
var chosen = fieldCentric;
return chosen
if (fieldCentricEnabled) {
return fieldCentric
.withVelocityX(vx)
.withVelocityY(vy)
.withRotationalRate(vomega);
@@ -231,16 +166,12 @@ public class DriveControls {
.withVelocityY(vy)
.withRotationalRate(vomega);
}
}, () -> fieldCentricEnabled ||
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench
? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
}, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
}
/**
*
* 输出移动到距离两个目标之间较近的那个点的位移
* @return 位移 有正负
* 杈撳嚭绉诲姩鍒拌窛绂讳袱涓洰鏍囦箣闂磋緝杩戠殑閭ｄ釜鐐圭殑浣嶇Щ
* @return 浣嶇Щ 鏈夋璐?
*/
public double calculateDifferenceToTwoTarget(double current, double target1, double target2){
double x1 = target1 - current;
@@ -248,16 +179,16 @@ public class DriveControls {
return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
}
public double[] calculateDistanceAndRotationToHub() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
Pose2d currentPose = drivetrain.getState().Pose; // 鑾峰彇鏈哄櫒浜哄綋鍓嶇殑浣嶇疆鍜岃搴?
double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY;
// 计算目标角度（相对于场地坐标系）
// 璁＄畻鐩爣瑙掑害锛堢浉瀵逛簬鍦哄湴鍧愭爣绯伙級
double deltaX = targetX - currentPose.getX();
double deltaY = targetY - currentPose.getY();
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 杞崲涓哄害
// 当前机器人角度
// 褰撳墠鏈哄櫒浜鸿搴?
double currentAngle = currentPose.getRotation().getDegrees();
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
@@ -267,26 +198,26 @@ public class DriveControls {
angleDifference += 360;
}
// 计算距离
// 璁＄畻璺濈
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
// 计算距离和旋转角度
// 璁＄畻璺濈鍜屾棆杞搴?
double[] distanceAndRotation = new double[]{distance, angleDifference};
// 返回距离和角度差
// 杩斿洖璺濈鍜岃搴﹀樊
return distanceAndRotation;
}
public double[] calculateDistanceAndRotationToPassBall() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 目标 X 坐标
double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标
double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标
Pose2d currentPose = drivetrain.getState().Pose; // 鑾峰彇鏈哄櫒浜哄綋鍓嶇殑浣嶇疆鍜岃搴?
double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 鐩爣 X 鍧愭爣
double targetY1 = Constants.Field.PassingBallPosY1; // 鐩爣 Y 鍧愭爣
double targetY2 = Constants.Field.PassingBallPosY2; // 鐩爣 Y 鍧愭爣
// 计算目标角度（相对于场地坐标系）
// 璁＄畻鐩爣瑙掑害锛堢浉瀵逛簬鍦哄湴鍧愭爣绯伙級
double deltaX = targetX - currentPose.getX();
double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 杞崲涓哄害
// 当前机器人角度
// 褰撳墠鏈哄櫒浜鸿搴?
double currentAngle = currentPose.getRotation().getDegrees();
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
@@ -296,12 +227,12 @@ public class DriveControls {
angleDifference += 360;
}
// 计算距离
// 璁＄畻璺濈
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
// 计算距离和旋转角度
// 璁＄畻璺濈鍜屾棆杞搴?
double[] distanceAndRotation = new double[]{distance, angleDifference};
// 返回距离和角度差
// 杩斿洖璺濈鍜岃搴﹀樊
return distanceAndRotation;
}
@@ -329,26 +260,26 @@ public class DriveControls {
} else {
vomega = error * Constants.AutoPositioning.TurningkP;
}
// 限制最大角速度
// 闄愬埗鏈€澶ц閫熷害
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
return vomega;
}
/**
* 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
* 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
* 鍙檺鍒垛€滃姞閫熲€濓紙骞呭€煎彉澶э級锛屼笉闄愬埗鈥滃噺閫熲€濓紙骞呭€煎彉灏忥級
* 杩欐牱鏉炬潌/鍒硅溅浼氭洿鍙婃椂锛屽悓鏃?boost 浠嶇劧鑳芥姂鍒剁數娴佸啿鍑?
*/
private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
// 如果正在减速（幅值变小），直接放行到目标值
// 濡傛灉姝ｅ湪鍑忛€燂紙骞呭€煎彉灏忥級锛岀洿鎺ユ斁琛屽埌鐩爣鍊?
if (Math.abs(target) < Math.abs(last)) {
limiter.reset(target); // 同步 limiter 内部状态，防止下一次突变
limiter.reset(target); // 鍚屾 limiter 鍐呴儴鐘舵€侊紝闃叉涓嬩竴娆＄獊鍙?
return target;
}
// 否则是加速，使用 limiter
// 鍚﹀垯鏄姞閫燂紝浣跨敤 limiter
return limiter.calculate(target);
}
/** 紧急停止：清零限速器，让速度输出立刻归零 */
/** 绱ф€ュ仠姝細娓呴浂闄愰€熷櫒锛岃閫熷害杈撳嚭绔嬪埢褰掗浂 */
public void emergencyStop() {
vxLimiter.reset(0.0);
vyLimiter.reset(0.0);
@@ -386,13 +317,13 @@ public class DriveControls {
return maxAngularRate;
}
public void setTeamColors() {
Optional<Alliance> ally = DriverStation.getAlliance(); // 获取队伍颜色
Optional<Alliance> ally = DriverStation.getAlliance(); // 鑾峰彇闃熶紞棰滆壊
if (ally.isPresent()) {
// 根据队伍颜色设置布尔值
// 鏍规嵁闃熶紞棰滆壊璁剧疆甯冨皵鍊?
if (ally.get() == Alliance.Red) {
isRedAlliance = true; // 红队
isRedAlliance = true; // 绾㈤槦
} else if (ally.get() == Alliance.Blue) {
isRedAlliance = false; // 蓝队
isRedAlliance = false; // 钃濋槦
}
} else {
isRedAlliance = true;
@@ -438,20 +369,20 @@ public class DriveControls {
switch (phase) {
case 0:
vx = amplitude; // 前
vx = amplitude; // 鍓?
vy = 0.0;
break;
case 1:
vx = 0.0;
vy = amplitude; // 右
vy = amplitude; // 鍙?
break;
case 2:
vx = -amplitude; // 后
vx = -amplitude; // 鍚?
vy = 0.0;
break;
case 3:
vx = 0.0;
vy = -amplitude; // 左
vy = -amplitude; // 宸?
break;
}
@@ -473,11 +404,11 @@ public class DriveControls {
public void pushDistanceData(){
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
// 将距离值写入 NetworkTable
// 灏嗚窛绂诲€煎啓鍏?NetworkTable
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
// 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新，
// 让自动程序里的射击门控 isAimed() 有效。
// 鑷姩闃舵榛樿椹鹃┒鍛戒护涓嶈繍琛岋紝杩欓噷鍚屾牱淇濇寔鐩爣鏈濆悜鏈€鏂帮紝
// 璁╄嚜鍔ㄧ▼搴忛噷鐨勫皠鍑婚棬鎺?isAimed() 鏈夋晥銆?
drivetrain.setTargetHeading(
drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
}
src/main/java/frc/robot/controls/OperatorControls.java
+11 -8
View File
@@ -2,6 +2,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.IntakeRotaterSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.Superstructure;
@@ -29,12 +30,16 @@ public class OperatorControls {
return superstructure.requestIntakeCommand();
}
public Command requestPrepShootCommand() {
return superstructure.requestPrepShootCommand();
public Command requestShootHubCommand() {
return superstructure.requestShootHubCommand();
}
public Command requestShootCommand() {
return superstructure.requestShootCommand();
public Command requestAimHubCommand() {
return superstructure.requestAimHubCommand();
}
public Command requestPassBallCommand() {
return superstructure.requestPassBallCommand();
}
public Command requestEjectCommand() {
@@ -73,10 +78,8 @@ public class OperatorControls {
// 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。
public Command autoShootToHubCommand() {
return Commands.sequence(
superstructure.requestPrepShootCommand(),
Commands.waitUntil(superstructure::canShoot).withTimeout(1.5),
superstructure.requestShootCommand(),
Commands.waitSeconds(1.2),
superstructure.requestShootHubCommand(),
Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
superstructure.requestIdleCommand()
);
}
src/main/java/frc/robot/subsystems/Superstructure.java
+69 -44
View File
@@ -23,8 +23,9 @@ public class Superstructure extends SubsystemBase {
public enum WantedState {
IDLE,
INTAKE,
PREP_SHOOT,
SHOOT,
SHOOT_HUB,
AIM_HUB,
PASS_BALL,
EJECT,
MANUAL
}
@@ -34,11 +35,8 @@ public class Superstructure extends SubsystemBase {
DISABLED,
STOWED,
INTAKING,
SPINNING_UP,
AIMING,
READY_TO_SHOOT,
PREP_SHOOT,
SHOOTING,
CLEANUP,
EJECTING,
MANUAL_OVERRIDE
}
@@ -56,6 +54,7 @@ public class Superstructure extends SubsystemBase {
private double stateStartTimestamp = Timer.getFPGATimestamp();
private double flywheelSpeedOffsetRps = 0.0;
private double backboardPositionOffset = 0.0;
private boolean shotCompletedThisRequest = false;
public Superstructure(
ShooterSubsystem shooter,
@@ -85,12 +84,26 @@ public class Superstructure extends SubsystemBase {
wantedState = WantedState.INTAKE;
}
public void requestPrepShoot() {
wantedState = WantedState.PREP_SHOOT;
public void requestShootHub() {
if (wantedState != WantedState.SHOOT_HUB) {
shotCompletedThisRequest = false;
}
wantedState = WantedState.SHOOT_HUB;
}
public void requestAimHub() {
wantedState = WantedState.AIM_HUB;
}
public void requestPassBall() {
if (wantedState != WantedState.PASS_BALL) {
shotCompletedThisRequest = false;
}
wantedState = WantedState.PASS_BALL;
}
public void requestShoot() {
wantedState = WantedState.SHOOT;
requestShootHub();
}
public void requestEject() {
@@ -109,12 +122,20 @@ public class Superstructure extends SubsystemBase {
return Commands.runOnce(this::requestIntake, this);
}
public Command requestPrepShootCommand() {
return Commands.runOnce(this::requestPrepShoot, this);
public Command requestShootHubCommand() {
return Commands.runOnce(this::requestShootHub, this);
}
public Command requestAimHubCommand() {
return Commands.runOnce(this::requestAimHub, this);
}
public Command requestPassBallCommand() {
return Commands.runOnce(this::requestPassBall, this);
}
public Command requestShootCommand() {
return Commands.runOnce(this::requestShoot, this);
return requestShootHubCommand();
}
public Command requestEjectCommand() {
@@ -133,6 +154,10 @@ public class Superstructure extends SubsystemBase {
return systemState;
}
public boolean isShootCycleComplete() {
return shotCompletedThisRequest;
}
/* ====================== */
/* 射击门控 */
/* ====================== */
@@ -188,8 +213,9 @@ public class Superstructure extends SubsystemBase {
switch (wantedState) {
case IDLE -> handleIdle();
case INTAKE -> handleIntake();
case PREP_SHOOT -> handlePrepShoot(false);
case SHOOT -> handlePrepShoot(true);
case SHOOT_HUB -> handleShootHub();
case AIM_HUB -> handleAimHub();
case PASS_BALL -> handlePassBall();
case EJECT -> handleEject();
case MANUAL -> handleManual();
}
@@ -204,6 +230,7 @@ public class Superstructure extends SubsystemBase {
shooter.stopFlywheel();
shooter.stopShooterConveyor();
shooter.holdBackboardAt(0.0);
setSystemState(SystemState.STOWED);
}
private void handleIntake() {
@@ -218,24 +245,39 @@ public class Superstructure extends SubsystemBase {
setSystemState(SystemState.INTAKING);
}
private void handlePrepShoot(boolean feedWhenReady) {
if (systemState != SystemState.SPINNING_UP
&& systemState != SystemState.AIMING
&& systemState != SystemState.READY_TO_SHOOT
&& systemState != SystemState.SHOOTING
&& systemState != SystemState.CLEANUP) {
setSystemState(SystemState.SPINNING_UP);
}
private void handleAimHub() {
intakeRoller.stop();
conveyor.stop();
shooter.stopFlywheel();
shooter.stopShooterConveyor();
shooter.holdBackboardAt(0.0);
setSystemState(SystemState.STOWED);
}
intakeRotater.lowerForMatch();
private void handleShootHub() {
double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
handlePreparedFeed(targetRps, backboardPosition);
}
private void handlePassBall() {
double targetRps = Constants.Superstructure.passBallFlywheelRps + flywheelSpeedOffsetRps;
double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
handlePreparedFeed(targetRps, backboardPosition);
}
private void handlePreparedFeed(double targetRps, double backboardPosition) {
if (systemState != SystemState.PREP_SHOOT
&& systemState != SystemState.SHOOTING) {
setSystemState(SystemState.PREP_SHOOT);
}
intakeRoller.stop();
conveyor.stop();
shooter.stopShooterConveyor();
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
// 但不会绕过 Superstructure 状态机。
double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
shooter.setFlywheelVelocity(targetRps);
shooter.holdBackboardAt(Constants.Superstructure.backboardShootPosition + backboardPositionOffset);
shooter.holdBackboardAt(backboardPosition);
if (systemState == SystemState.SHOOTING) {
// 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
@@ -245,17 +287,8 @@ public class Superstructure extends SubsystemBase {
shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
setSystemState(SystemState.CLEANUP);
}
return;
}
if (systemState == SystemState.CLEANUP) {
intakeRotater.lowerForMatch();
conveyor.stop();
shooter.stopShooterConveyor();
if (timeInState() > Constants.Superstructure.cleanupSeconds) {
setSystemState(SystemState.STOWED);
shotCompletedThisRequest = true;
setSystemState(SystemState.PREP_SHOOT);
}
return;
}
@@ -266,14 +299,6 @@ public class Superstructure extends SubsystemBase {
// 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
if (canShoot()) {
setSystemState(SystemState.READY_TO_SHOOT);
} else if (shooter.atSpeed()) {
setSystemState(SystemState.AIMING);
} else {
setSystemState(SystemState.SPINNING_UP);
}
if (feedWhenReady && systemState == SystemState.READY_TO_SHOOT) {
setSystemState(SystemState.SHOOTING);
}
}