From dd83652411aa231c959820e3407e2ff349284cd7 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Tue, 5 May 2026 16:49:20 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=89=E5=85=A8=E6=BC=94=E7=A4=BA=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 14 +++++++- src/main/java/frc/robot/RobotContainer.java | 15 ++++++-- .../frc/robot/controls/DriveControls.java | 35 +++++++++++++++++-- .../subsystems/CommandSwerveDrivetrain.java | 22 ++++++++++-- 4 files changed, 76 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 93516d6..0d52aaa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -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; } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f66f1d..312cac9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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(); } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 85eb28d..96377e3 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -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( diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 585e787..364f0d6 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -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 -> {