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-05-05 16:49:20 +08:00
parent af1095cf50
commit dd83652411
4 changed files with 76 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/Constants.java
+13 -1
View File
@@ -6,6 +6,18 @@ import com.ctre.phoenix6.configs.Slot0Configs;
import edu.wpi.first.math.geometry.Pose2d;
public final class Constants {
public static final class DemoMode {
public static final boolean ENABLED = true;
public static final double normalDriveScale = 0.12;
public static final double normalTurnScale = 0.20;
public static final double boostDriveScale = 0.25;
public static final double boostTurnScale = 0.30;
public static final double maxLinearSpeedMetersPerSecond = 1.20;
public static final double maxAngularRateRadiansPerSecond = 0.80;
}
public static final boolean LEDUsing = false;
public enum RobotStatus{
Stopped,
@@ -169,4 +181,4 @@ public final class Constants {
slot0Configs.kA = 0.0;
}
}
}
}
src/main/java/frc/robot/RobotContainer.java
+12 -3
View File
@@ -139,7 +139,13 @@ public class RobotContainer {
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
// Start：强行使用metaTag2全场定位
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
if (Constants.DemoMode.ENABLED) {
controllerlower.start()
.onTrue(Commands.runOnce(driveControls::emergencyStop)
.andThen(robotStatusManager.setStatusCommand(RobotStatus.Stopped)))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
} else {
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
System.out.println("Try using shooter for metatag2");
@@ -172,6 +178,7 @@ public class RobotContainer {
controllerlower.a()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
}
}
private void configueShooter(){
@@ -181,7 +188,7 @@ public class RobotContainer {
controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
controllerupper.start().onTrue(shooterControls.resetBackboardCommand());
}
@@ -190,7 +197,9 @@ public class RobotContainer {
}
public Command getAutonomousCommand() {
if (Constants.DemoMode.ENABLED) {
return Commands.none();
}
return autoChooser.getSelected();
}
src/main/java/frc/robot/controls/DriveControls.java
+32 -3
View File
@@ -107,9 +107,18 @@ public class DriveControls {
SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
double turnScale = boostEnabled ? boostTurnScale : normalTurnScale;
if (Constants.DemoMode.ENABLED) {
driveScale = boostEnabled ? Constants.DemoMode.boostDriveScale : Constants.DemoMode.normalDriveScale;
turnScale = boostEnabled ? Constants.DemoMode.boostTurnScale : Constants.DemoMode.normalTurnScale;
}
double vx = -driver.getLeftY() * maxSpeed * driveScale;
double vy = -driver.getLeftX() * maxSpeed * driveScale;
double vomega = -driver.getRightX() * maxAngularRate * turnScale;
if (Constants.DemoMode.ENABLED) {
vx = clamp(vx, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
vy = clamp(vy, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
vomega = clamp(vomega, -Constants.DemoMode.maxAngularRateRadiansPerSecond, Constants.DemoMode.maxAngularRateRadiansPerSecond);
}
// ====== execute 内替换 limiter：只限加速、不限减速 ======
double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
@@ -130,7 +139,15 @@ public class DriveControls {
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
switch (robotStatusManager.getStatus()) {
RobotStatus currentStatus = robotStatusManager.getStatus();
if (Constants.DemoMode.ENABLED
&& currentStatus != RobotStatus.Stopped
&& currentStatus != RobotStatus.AllTelop
&& currentStatus != RobotStatus.Climbing) {
currentStatus = RobotStatus.AllTelop;
}
switch (currentStatus) {
case Stopped:
vx = 0;
vy = 0;
@@ -212,8 +229,9 @@ public class DriveControls {
autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
boolean isAutoLike =
!Constants.DemoMode.ENABLED && (
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
robotStatusManager.getStatus() == RobotStatus.CrossingTrench);
if (fieldCentricEnabled || isAutoLike) {
@@ -231,8 +249,9 @@ public class DriveControls {
.withRotationalRate(vomega);
}
}, () -> fieldCentricEnabled ||
(!Constants.DemoMode.ENABLED && (
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench
robotStatusManager.getStatus() == RobotStatus.CrossingTrench))
? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
}
@@ -347,6 +366,10 @@ public class DriveControls {
return limiter.calculate(target);
}
private double clamp(double value, double min, double max) {
return Math.max(min, Math.min(max, value));
}
/** 紧急停止：清零限速器，让速度输出立刻归零 */
public void emergencyStop() {
vxLimiter.reset(0.0);
@@ -416,6 +439,9 @@ public class DriveControls {
// }
// }
public Command autoAimCommand() {
if (Constants.DemoMode.ENABLED) {
return Commands.runOnce(drivetrain::stop, drivetrain);
}
return drivetrain.applyRequest(() -> {
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
@@ -431,6 +457,9 @@ public class DriveControls {
}, drivetrain));
}
public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
if (Constants.DemoMode.ENABLED) {
return Commands.runOnce(drivetrain::stop, drivetrain);
}
Timer timer = new Timer();
return Commands.sequence(
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+19 -3
View File
@@ -162,6 +162,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
public void forceUsingLimelightmt2() {
if (Constants.DemoMode.ENABLED) {
return;
}
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
@@ -174,6 +177,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
public void forceUsingLimelightmt2WithllName(String llString) {
if (Constants.DemoMode.ENABLED) {
return;
}
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
@@ -426,8 +432,16 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
Pose2d pose = getState().Pose;
m_field.setRobotPose(pose);
posePub.set(pose);
shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose);
intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose);
LimelightHelpers.PoseEstimate shooterEstimate =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
LimelightHelpers.PoseEstimate intakerEstimate =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker);
if (shooterEstimate != null && shooterEstimate.tagCount > 0) {
shooter_ll_field.setRobotPose(shooterEstimate.pose);
}
if (intakerEstimate != null && intakerEstimate.tagCount > 0) {
intaker_ll_field.setRobotPose(intakerEstimate.pose);
}
driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
@@ -448,7 +462,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
0, 0, 0, 0, 0
);
fuseLimelightmt2();
if (!Constants.DemoMode.ENABLED) {
fuseLimelightmt2();
}
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
DriverStation.getAlliance().ifPresent(allianceColor -> {