396 lines
16 KiB
Java
396 lines
16 KiB
Java
package frc.robot.controls;
|
||
import static edu.wpi.first.units.Units.MetersPerSecond;
|
||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||
|
||
import edu.wpi.first.math.filter.SlewRateLimiter;
|
||
import edu.wpi.first.math.geometry.Pose2d;
|
||
import edu.wpi.first.math.geometry.Rotation2d;
|
||
import edu.wpi.first.networktables.NetworkTable;
|
||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||
import edu.wpi.first.wpilibj2.command.Command;
|
||
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 java.util.Optional;
|
||
import edu.wpi.first.wpilibj.DriverStation;
|
||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||
|
||
public class DriveControls {
|
||
public boolean isRedAlliance;
|
||
|
||
private final CommandSwerveDrivetrain drivetrain;
|
||
private final CommandXboxController driver;
|
||
|
||
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）
|
||
private double boostDriveScale = 1.0;
|
||
private double boostTurnScale = 1.0;
|
||
|
||
private boolean boostEnabled = false;
|
||
|
||
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 double lastVx = 0.0;
|
||
private double lastVy = 0.0;
|
||
private double lastOmega = 0.0;
|
||
|
||
private NetworkTable autoControlNetworkTable;
|
||
|
||
private final RobotStatusManager robotStatusManager;
|
||
public void setBoostEnabled(boolean enabled) {
|
||
boostEnabled = enabled;
|
||
}
|
||
|
||
// 新增：当前是否使用场地坐标系（默认 true）
|
||
private boolean fieldCentricEnabled = true;
|
||
|
||
private final SwerveRequest.FieldCentric fieldCentric =
|
||
new SwerveRequest.FieldCentric()
|
||
.withDeadband(maxSpeed * 0.05)
|
||
.withRotationalDeadband(maxAngularRate * 0.05)
|
||
.withDriveRequestType(DriveRequestType.Velocity);
|
||
|
||
private final SwerveRequest.FieldCentric fieldCentricAuto =
|
||
new SwerveRequest.FieldCentric()
|
||
.withDeadband(maxSpeed * 0.05)
|
||
.withRotationalDeadband(maxAngularRate * 0.05)
|
||
.withDriveRequestType(DriveRequestType.Velocity)
|
||
.withForwardPerspective(SwerveRequest.ForwardPerspectiveValue.BlueAlliance);
|
||
|
||
private final SwerveRequest.RobotCentric robotCentric =
|
||
new SwerveRequest.RobotCentric()
|
||
.withDeadband(maxSpeed * 0.05)
|
||
.withRotationalDeadband(maxAngularRate * 0.05)
|
||
.withDriveRequestType(DriveRequestType.Velocity);
|
||
|
||
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
|
||
this.drivetrain = drivetrain;
|
||
this.driver = driver;
|
||
this.robotStatusManager = rsm;
|
||
autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
|
||
}
|
||
|
||
public void setFieldCentricEnabled(boolean enabled) {
|
||
fieldCentricEnabled = enabled;
|
||
}
|
||
|
||
public boolean isFieldCentricEnabled() {
|
||
return fieldCentricEnabled;
|
||
}
|
||
|
||
/** 底盘的默认驾驶命令（teleop 期间持续执行） */
|
||
public Command defaultDriveCommand() {
|
||
return drivetrain.applyRequest(() -> {
|
||
SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().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);
|
||
|
||
lastVx = limitedVx;
|
||
lastVy = limitedVy;
|
||
lastOmega = limitedOmega;
|
||
|
||
vx = limitedVx;
|
||
vy = limitedVy;
|
||
vomega = limitedOmega;
|
||
|
||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||
// 将距离值写入 NetworkTable
|
||
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
|
||
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
|
||
|
||
switch (robotStatusManager.getStatus()) {
|
||
case Stopped:
|
||
vx = 0;
|
||
vy = 0;
|
||
vomega = 0;
|
||
break;
|
||
case PassingBall:
|
||
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
|
||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
|
||
break;
|
||
case AutoAimming:
|
||
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
|
||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
|
||
break;
|
||
case AllTelop:
|
||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
|
||
break;
|
||
case Climbing:
|
||
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
|
||
);
|
||
}
|
||
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
|
||
);
|
||
}
|
||
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;
|
||
return chosen
|
||
.withVelocityX(vx)
|
||
.withVelocityY(vy)
|
||
.withRotationalRate(vomega);
|
||
} else {
|
||
return robotCentric
|
||
.withVelocityX(vx)
|
||
.withVelocityY(vy)
|
||
.withRotationalRate(vomega);
|
||
}
|
||
}, () -> fieldCentricEnabled ||
|
||
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
|
||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench
|
||
? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
|
||
}
|
||
|
||
/**
|
||
*
|
||
* 输出移动到距离两个目标之间较近的那个点的位移
|
||
* @return 位移 有正负
|
||
*/
|
||
public double calculateDifferenceToTwoTarget(double current, double target1, double target2){
|
||
double x1 = target1 - current;
|
||
double x2 = target2 - current;
|
||
return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
|
||
}
|
||
public double[] calculateDistanceAndRotationToHub() {
|
||
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
|
||
double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标
|
||
double targetY = Constants.Field.RedHubPositionY; // 目标 Y 坐标
|
||
|
||
// 计算目标角度（相对于场地坐标系）
|
||
double deltaX = targetX - currentPose.getX();
|
||
double deltaY = targetY - currentPose.getY();
|
||
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]
|
||
if (angleDifference > 180) {
|
||
angleDifference -= 360;
|
||
} else if (angleDifference < -180) {
|
||
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 = Constants.Field.PassingBallPosX; // 目标 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 currentAngle = currentPose.getRotation().getDegrees();
|
||
double angleDifference = targetAngle - currentAngle;
|
||
// Normalize angle difference to the range [-180, 180]
|
||
if (angleDifference > 180) {
|
||
angleDifference -= 360;
|
||
} else if (angleDifference < -180) {
|
||
angleDifference += 360;
|
||
}
|
||
|
||
// 计算距离
|
||
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
|
||
// 计算距离和旋转角度
|
||
double[] distanceAndRotation = new double[]{distance, angleDifference};
|
||
|
||
// 返回距离和角度差
|
||
return distanceAndRotation;
|
||
}
|
||
|
||
public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
|
||
double calDifference = angleDifference;
|
||
|
||
// Optional: Apply teleop offset if enabled
|
||
if (Constants.AutoPositioning.teleopOffsetEnabled) {
|
||
double teleopOffset = -driver.getRightX();
|
||
calDifference += teleopOffset * Constants.AutoPositioning.teleopOffsetkP;
|
||
}
|
||
|
||
// If angle difference is within threshold, don't rotate
|
||
if (Math.abs(calDifference) > 1.0) {
|
||
return calDifference * Constants.AutoPositioning.autoRotationkP;
|
||
} else {
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
private double calculatevomegaFromRotationAngle(double error) {
|
||
double vomega;
|
||
if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
|
||
vomega = 0.0;
|
||
} else {
|
||
vomega = error * Constants.AutoPositioning.TurningkP;
|
||
}
|
||
// 限制最大角速度
|
||
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
|
||
return vomega;
|
||
}
|
||
|
||
/**
|
||
* 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
|
||
* 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
|
||
*/
|
||
private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
|
||
// 如果正在减速（幅值变小），直接放行到目标值
|
||
if (Math.abs(target) < Math.abs(last)) {
|
||
limiter.reset(target); // 同步 limiter 内部状态，防止下一次突变
|
||
return target;
|
||
}
|
||
// 否则是加速，使用 limiter
|
||
return limiter.calculate(target);
|
||
}
|
||
|
||
/** 紧急停止：清零限速器，让速度输出立刻归零 */
|
||
public void emergencyStop() {
|
||
vxLimiter.reset(0.0);
|
||
vyLimiter.reset(0.0);
|
||
omegaLimiter.reset(0.0);
|
||
|
||
lastVx = 0.0;
|
||
lastVy = 0.0;
|
||
lastOmega = 0.0;
|
||
}
|
||
|
||
public Command idleCommand() {
|
||
final var idle = new SwerveRequest.Idle();
|
||
return drivetrain.applyRequest(() -> idle).ignoringDisable(true);
|
||
}
|
||
|
||
public Command brakeWhileHeld() {
|
||
final var brake = new SwerveRequest.SwerveDriveBrake();
|
||
return drivetrain.applyRequest(() -> brake);
|
||
}
|
||
|
||
|
||
public SwerveRequest.SwerveDriveBrake brakeRequest() {
|
||
return new SwerveRequest.SwerveDriveBrake();
|
||
}
|
||
|
||
public SwerveRequest.Idle idleRequest() {
|
||
return new SwerveRequest.Idle();
|
||
}
|
||
|
||
public double getMaxSpeed() {
|
||
return maxSpeed;
|
||
}
|
||
|
||
public double getMaxAngularRate() {
|
||
return maxAngularRate;
|
||
}
|
||
public void setTeamColors() {
|
||
Optional<Alliance> ally = DriverStation.getAlliance(); // 获取队伍颜色
|
||
if (ally.isPresent()) {
|
||
// 根据队伍颜色设置布尔值
|
||
if (ally.get() == Alliance.Red) {
|
||
isRedAlliance = true; // 红队
|
||
} else if (ally.get() == Alliance.Blue) {
|
||
isRedAlliance = false; // 蓝队
|
||
}
|
||
} else {
|
||
isRedAlliance = true;
|
||
}
|
||
if (isRedAlliance) {
|
||
System.out.println("Robot is on the Red Alliance.");
|
||
} else {
|
||
System.out.println("Robot is on the Blue Alliance.");
|
||
}
|
||
}
|
||
public Command getAllianceColorCommand() {
|
||
return Commands.runOnce(this::setTeamColors);
|
||
}
|
||
|
||
}
|