改个射速

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 = 2.4;
+        public static final double maxAngularRateRadiansPerSecond = 1.6;
+    }
+
     public static final boolean LEDUsing = false;
     public enum RobotStatus{
         Stopped,
@@ -17,7 +29,7 @@ public final class Constants {
         CrossingTrench
     }
     public class Shooter {
-        public static final double conveyorSpeed = 150;
+        public static final double conveyorSpeed = 80;
         public static final double conveyorSpeedForAuto = 10;
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
@@ -80,7 +92,7 @@ public final class Constants {
     public class Limelight {
         public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
         public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
-        public static final Boolean UsingMetaTag2 = true; // 是否使用 MetaTag2（如果你们的 pipeline 配置了这个功能的话）
+        public static final Boolean UsingMetaTag2 = false; // Use Limelight's own pose/yaw estimate instead of gyro-assisted MegaTag2.
         public static final int climbPipelineIndex = 8;
         public static final int locatePipelineIndex = 0;
 

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

@@ -36,9 +36,9 @@ public class RobotContainer {
   /*         手柄            */
   /* ====================== */
 
-  // 使用2个 Xbox 手柄（端口 0,1）
+  // Demo mode uses one Xbox controller on port 0.
   private final CommandXboxController controllerlower = new CommandXboxController(0);
-  private final CommandXboxController controllerupper = new CommandXboxController(1);
+  private final CommandXboxController controllerupper = controllerlower;
 
   /* ====================== */
   /*         子系统           */
@@ -124,6 +124,8 @@ public class RobotContainer {
     // Disabled 时进入 idle（防止模块乱动）
     RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
 
+    if (!Constants.DemoMode.ENABLED) {
+
     
     //左保险：按住刹车
     controllerlower.leftBumper()
@@ -173,6 +175,7 @@ public class RobotContainer {
     .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
     .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
     }
+  }
 
   private void configueShooter(){
     shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
@@ -181,7 +184,13 @@ 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()));
+    if (Constants.DemoMode.ENABLED) {
+      controllerupper.rightTrigger()
+          .onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming))
+          .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    } else {
+      controllerupper.start().onTrue(shooterControls.resetBackboardCommand());
+    }
     
   }
 
@@ -190,7 +199,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/controls/IntakerControls.java

@@ -81,6 +81,12 @@ public class IntakerControls {
 
         IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
 
+        boolean demoModeEnabled = Constants.DemoMode.ENABLED;
+        if (demoModeEnabled) {
+            IntakerSubsystem.stopIntakerotater();
+            return;
+        }
+
         if(controller.getLeftY() < -0.5){
                     IntakerSubsystem.IntakerUpNonStop();
             } else if(controller.getLeftY() > 0.5){

src/main/java/frc/robot/controls/ShooterControls.java

@@ -48,7 +48,7 @@ public class ShooterControls {
                         shooterSubsystem.outputBackboard();
                     }  
                     shooterSubsystem.setFlywheelSpeedByRPS(0);
-                    if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
+                    if(!Constants.DemoMode.ENABLED && controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
                     else{shooterSubsystem.setConveyorSpeedByRPS(0);}
                     break;
                 case PassingBall:
@@ -81,9 +81,11 @@ public class ShooterControls {
                 case AutoAimming:
                     var x = shooterSubsystem.getDistanceToHub();
                     x = Math.max(1.2, Math.min(4.5, x));
-                    flywheelSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
+                    //flywheelSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
+                    flywheelSpeed = 60;
                     flywheelSpeed += flywheelSpeedOffset;
-                    backboardPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
+                    //backboardPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
+                    backboardPosition = 500;
                     backboardPosition += backboardPositionOffset;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {

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