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/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])));
}