安全演示版

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,

src/main/java/frc/robot/RobotContainer.java

@@ -139,6 +139,12 @@ public class RobotContainer {
     controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
     
     // Start：强行使用metaTag2全场定位
+    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);
@@ -173,6 +179,7 @@ public class RobotContainer {
     .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
     .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
     }
+  }
 
   private void configueShooter(){
     shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
@@ -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

@@ -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

@@ -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);
+        LimelightHelpers.PoseEstimate shooterEstimate =
-        intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose);
+            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
         );
 
+        if (!Constants.DemoMode.ENABLED) {
             fuseLimelightmt2();
+        }
 
         if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
             DriverStation.getAlliance().ifPresent(allianceColor -> {