键位定义 手动协作 bug没改完

elastic-layout.json

@@ -539,7 +539,7 @@
       }
     },
     {
-      "name": "SwervePID",
+      "name": "PID",
       "grid_layout": {
         "layouts": [],
         "containers": [
@@ -669,9 +669,9 @@
           },
           {
             "title": "flywheelEnableRunning",
-            "x": 640.0,
+            "x": 896.0,
-            "y": 256.0,
+            "y": 0.0,
-            "width": 128.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Toggle Switch",
             "properties": {
@@ -683,8 +683,8 @@
           {
             "title": "flywheelRunningSpeedRPS",
             "x": 640.0,
-            "y": 384.0,
+            "y": 128.0,
-            "width": 128.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
             "properties": {
@@ -697,8 +697,8 @@
           {
             "title": "flywheelEnable",
             "x": 640.0,
-            "y": 128.0,
+            "y": 0.0,
-            "width": 128.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Toggle Switch",
             "properties": {
@@ -706,6 +706,48 @@
               "period": 0.06,
               "data_type": "boolean"
             }
+          },
+          {
+            "title": "flywheelkV",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/flywheel/flywheelkV",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "flywheelkS",
+            "x": 640.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/flywheel/flywheelkS",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "flywheelkA",
+            "x": 640.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Tuning/flywheel/flywheelkA",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
           }
         ]
       }
@@ -730,10 +772,10 @@
           },
           {
             "title": "limelight-shooter",
-            "x": 384.0,
+            "x": 768.0,
-            "y": 0.0,
+            "y": 512.0,
-            "width": 256.0,
+            "width": 640.0,
-            "height": 256.0,
+            "height": 384.0,
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/limelight-shooter",
@@ -1070,7 +1112,7 @@
             "x": 1152.0,
             "y": 0.0,
             "width": 384.0,
-            "height": 384.0,
+            "height": 256.0,
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/limelight-shooter",
@@ -1160,7 +1202,7 @@
           {
             "title": "Field",
             "x": 1152.0,
-            "y": 384.0,
+            "y": 256.0,
             "width": 384.0,
             "height": 256.0,
             "type": "Field",
@@ -1237,56 +1279,56 @@
             }
           },
           {
-            "title": "backboardTargetRate",
+            "title": "backboardTargetPosition",
             "x": 1024.0,
             "y": 256.0,
             "width": 128.0,
             "height": 128.0,
             "type": "Text Display",
             "properties": {
-              "topic": "/Shooter/backboardTargetRate",
+              "topic": "/ShooterControl/backboardTargetPosition",
               "period": 0.06,
               "data_type": "double",
               "show_submit_button": true
             }
           },
           {
-            "title": "conveyorPIDReference",
+            "title": "flywheelTargetSpeed",
-            "x": 512.0,
-            "y": 256.0,
-            "width": 128.0,
-            "height": 128.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/Shooter/conveyorPIDReference",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": true
-            }
-          },
-          {
-            "title": "flywheelPIDReference",
             "x": 768.0,
             "y": 256.0,
             "width": 128.0,
             "height": 128.0,
             "type": "Text Display",
             "properties": {
-              "topic": "/Shooter/flywheelPIDReference",
+              "topic": "/ShooterControl/flywheelTargetSpeed",
               "period": 0.06,
               "data_type": "double",
               "show_submit_button": true
             }
           },
           {
-            "title": "conveyorPIDReference",
+            "title": "conveyerTargetSpeed",
+            "x": 512.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/ShooterControl/conveyerTargetSpeed",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "conveyerTargetSpeed",
             "x": 384.0,
             "y": 384.0,
             "width": 256.0,
             "height": 256.0,
             "type": "Graph",
             "properties": {
-              "topic": "/Shooter/conveyorPIDReference",
+              "topic": "/ShooterControl/conveyerTargetSpeed",
               "period": 0.033,
               "data_type": "double",
               "time_displayed": 5.0,
@@ -1295,14 +1337,14 @@
             }
           },
           {
-            "title": "flywheelPIDReference",
+            "title": "flywheelTargetSpeed",
             "x": 640.0,
             "y": 384.0,
             "width": 256.0,
             "height": 256.0,
             "type": "Graph",
             "properties": {
-              "topic": "/Shooter/flywheelPIDReference",
+              "topic": "/ShooterControl/flywheelTargetSpeed",
               "period": 0.033,
               "data_type": "double",
               "time_displayed": 5.0,
@@ -1311,20 +1353,140 @@
             }
           },
           {
-            "title": "backboardTargetRate",
+            "title": "backboardTargetPosition",
             "x": 896.0,
             "y": 384.0,
             "width": 256.0,
             "height": 256.0,
             "type": "Graph",
             "properties": {
-              "topic": "/Shooter/backboardTargetRate",
+              "topic": "/ShooterControl/backboardTargetPosition",
               "period": 0.033,
               "data_type": "double",
               "time_displayed": 5.0,
               "color": 4278238420,
               "line_width": 2.0
             }
+          },
+          {
+            "title": "Usingll",
+            "x": 1152.0,
+            "y": 512.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Positioning/Usingll",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "robotStatus",
+            "x": 0.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/robotStatus",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": true
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Tab 7",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "ActualVOmega",
+            "x": 256.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AutoControl/ActualVOmega",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "ActualVX",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AutoControl/ActualVX",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "ActualVY",
+            "x": 128.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AutoControl/ActualVY",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "BumpDifference",
+            "x": 384.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AutoControl/BumpDifference",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "TrenchDifference",
+            "x": 384.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AutoControl/TrenchDifference",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "robotStatus",
+            "x": 0.0,
+            "y": 256.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/robotStatus",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": true
+            }
           }
         ]
       }

src/main/java/frc/robot/Constants.java

@@ -5,6 +5,14 @@ import edu.wpi.first.math.geometry.Pose2d;
 
 public final class Constants {
     public static final boolean LEDUsing = false;
+    public enum RobotStatus{
+        Stopped,
+        AllTelop,
+        AutoAimming,
+        Climbing,
+        CrossingBump,
+        CrossingTrench
+    }
     public class Shooter {
         public static final double conveyorSpeed = 20;
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
@@ -35,15 +43,16 @@ public final class Constants {
         public static final double positionMid  = 1600;  // 中距离背板角度
         public static final double postionFar  = 3200;  // 远距离背板角度
     }
-    public class AutoRotation {
+    public class AutoPositioning {
-        public static final boolean autoRotationToHubEnabled = true;
+        public static final double autoRotationkP = 0.1;
-        public static final double autoRotationToHubkP = 0.12;
-        public static final boolean autoRotationForBump = false;
-        public static final double autoRotationForBumpkP = 0.2;
         public static final double autoRotationForBumpTargetDegrees = 45;
-
+        public static final double autoRotationForTrenchTargetDegrees = 0;
         public static final boolean teleopOffsetEnabled = true;
         public static final double teleopOffsetkP = 15;
+
+        public static final double autoPositioningkP = 0.1;
+        public static final double[] bumpY = {2.51,5.556};
+        public static final double[] trenchY = {0.639,7.43};
     }
 
     public class StartingPoints{

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

@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import frc.robot.Constants.RobotStatus;
 import frc.robot.controls.DriveControls;
 import frc.robot.controls.ShooterControls;
 import frc.robot.generated.TunerConstants;
@@ -41,12 +42,13 @@ public class RobotContainer {
   // LED 子系统
   private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72         
 
+  private final RobotStatusManager robotStatusManager = new RobotStatusManager();
   /* ====================== */
   /*     控制封装/日志        */
   /* ====================== */
 
-  private final DriveControls driveControls = new DriveControls(drivetrain, controller);
+  private final DriveControls driveControls = new DriveControls(drivetrain, controller,robotStatusManager );
-  private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller);
+  private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller,robotStatusManager);
 
   private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
   private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
@@ -55,6 +57,7 @@ public class RobotContainer {
 
   private final SendableChooser<Command> autoChooser;
 
+  
   public RobotContainer() {
     SignalLogger.enableAutoLogging(false);
     autoChooser = AutoBuilder.buildAutoChooser();
@@ -72,31 +75,46 @@ public class RobotContainer {
     Pose2d startingPose = Constants.StartingPoints.Red.Point2;
     drivetrain.resetPose(startingPose);
 
-    // 默认驾驶命令
+    // 默认驾驶命令 
+    /*
+     * LeftX 底盘左右
+     * LeftY 底盘前后  Bump/Trench偏移未完成
+     * RightX Aiming偏移    AllTelop模式下 底盘旋转
+     * 
+    */
     drivetrain.setDefaultCommand(driveControls.defaultDriveCommand());
 
     // Disabled 时进入 idle（防止模块乱动）
     RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
 
     
-    //左保险：按住刹车
+    // //左保险：按住刹车
-    controller.leftBumper()
+    // controller.leftBumper()
-      .onTrue(Commands.runOnce(driveControls::emergencyStop))
+    //   .onTrue(Commands.runOnce(driveControls::emergencyStop))
-      .whileTrue(driveControls.brakeWhileHeld());
+    //   .whileTrue(driveControls.brakeWhileHeld());
+    
+    // // 右保险：按住加速（Boost）
+    // controller.rightBumper()
+    //     .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
+    //     .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
 
-    // Back：重置场地坐标系
+    // Back：重置场地坐标系角度
     controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
     
-    // Start：切换场地/车体坐标系
+    // Start：强行使用metaTag2全场定位
-    controller.start().onTrue(Commands.runOnce(driveControls::toggleDriveFrame));
+    controller.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
 
-    // 右保险：按住加速（Boost）
+    //LM 按下时使用机器人坐标系
-    controller.rightBumper()
+    controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
-        .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
+    controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
-        .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
-
-    controller.a().whileTrue(drivetrain.runOnce(drivetrain::forceUsingLimelightMT2));
 
+    controller.rightStick().and(controller.leftBumper())
+    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
+    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    
+    controller.rightStick().and(controller.rightBumper())
+    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
+    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
   }
 
   private void configueShooter(){
@@ -107,7 +125,6 @@ public class RobotContainer {
     controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
     controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
   }
-
   /* ====================== */
   /*        LED 绑定          */
   /* ====================== */

src/main/java/frc/robot/RobotStatusManager.java

@@ -0,0 +1,25 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.RobotStatus;
+
+public class RobotStatusManager {
+    private RobotStatus robotStatus;
+    
+    public RobotStatusManager(){
+        this.robotStatus = RobotStatus.AllTelop;
+    }
+
+    public RobotStatus getStatus(){
+        return this.robotStatus;
+    }
+
+    public void setStatus(RobotStatus rs){
+        this.robotStatus = rs;
+    }
+
+    public Command setStatusCommand(RobotStatus rs){
+        return Commands.runOnce(()->setStatus(rs));
+    }
+}

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

@@ -8,10 +8,13 @@ import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 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.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;
 
@@ -43,6 +46,7 @@ public class DriveControls {
 
     private NetworkTable autoControlNetworkTable;
     
+    private final RobotStatusManager robotStatusManager;
     public void setBoostEnabled(boolean enabled) {
         boostEnabled = enabled;
     }
@@ -62,36 +66,34 @@ public class DriveControls {
         .withRotationalDeadband(maxAngularRate * 0.05)
         .withDriveRequestType(DriveRequestType.Velocity);
     
-    public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver) {
+    public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
         this.drivetrain = drivetrain;
         this.driver = driver;
+        this.robotStatusManager = rsm;
         autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
     }
     
-    public void toggleDriveFrame() {
+    public void setFieldCentricEnabled(boolean enabled) {
-        fieldCentricEnabled = !fieldCentricEnabled;
+        fieldCentricEnabled = enabled;
     }
     
     public boolean isFieldCentricEnabled() {
         return fieldCentricEnabled;
     }
-    public DriveMode getCurrentDriveMode() {
+
-        return fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC;
-    }
     /** 底盘的默认驾驶命令（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 omega = -driver.getRightX() * maxAngularRate * turnScale;
+        double vomega = -driver.getRightX() * maxAngularRate * turnScale;
-
         // ====== execute 内替换 limiter：只限加速、不限减速 ======
         double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
         double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
-        double limitedOmega = limitAccelOnly(omega, lastOmega, omegaLimiter);
+        double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter);
 
         lastVx = limitedVx;
         lastVy = limitedVy;
@@ -99,45 +101,100 @@ public class DriveControls {
 
         vx = limitedVx;
         vy = limitedVy;
-        omega = limitedOmega;
+        vomega = limitedOmega;
-        
+
         double[] distanceAndRotation = calculateDistanceAndRotationToHub();
         // 将距离值写入 NetworkTable
         autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
         autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
 
-        if(Constants.AutoRotation.autoRotationToHubEnabled) {
+        switch (robotStatusManager.getStatus()) {
-            double autoRotationPower = driver.getLeftTriggerAxis();
+            case Stopped:
-            if(autoRotationPower > 0.1) {
+                vx = 0;
-                autoControlNetworkTable.getEntry("autoRotationAvailable").setBoolean(true);
+                vy = 0;
+                vomega = 0;
+                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]);
+                vy = differenceBump * Constants.AutoPositioning.autoPositioningkP;
+                autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
+                //vomega = calculateRotationSpeedForBump();
+                break;
+            case CrossingTrench:
+                double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]);
+                vy = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
+                autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
+                //vomega = calculateRotationSpeedForTrench();
+                break;
+        }        
 
-                double angleDifference = distanceAndRotation[1];
+        autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
+        autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
+        autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
 
-                // Calculate rotation speed using the angle difference
+        if (fieldCentricEnabled || 
-                omega = calculateRotationSpeedFromRotationAngle(angleDifference, autoRotationPower);
+            robotStatusManager.getStatus() == RobotStatus.CrossingBump || 
-
+            robotStatusManager.getStatus() == RobotStatus.CrossingTrench) {
-                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(omega);
-            } else {
-                autoControlNetworkTable.getEntry("autoRotationAvailable").setBoolean(false);
-            }
-        }
-
-      
-
-        if (fieldCentricEnabled) {
             return fieldCentric
                 .withVelocityX(vx)
                 .withVelocityY(vy)
-                .withRotationalRate(omega);
+                .withRotationalRate(vomega);
         } else {
             return robotCentric
                 .withVelocityX(vx)
                 .withVelocityY(vy)
-                .withRotationalRate(omega);
+                .withRotationalRate(vomega);
         }
-        }, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
+        }, () -> 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 calculateRotationSpeedForTrench(){
+        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
+        double currentAngle = currentPose.getRotation().getDegrees();
+        double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度
+        double angleDifference = targetAngle - currentAngle;
+        // Normalize angle difference to the range [-180, 180]
+        if (angleDifference > 180) {
+            angleDifference -= 360;
+        } else if (angleDifference < -180) {
+            angleDifference += 360;
+        }
+        return calculateRotationSpeedFromRotationAngle(angleDifference);
+    }
+    public double calculateRotationSpeedForBump(){
+        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
+        double currentAngle = currentPose.getRotation().getDegrees();
+        double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度
+        double angleDifference = targetAngle - currentAngle;
+        // Normalize angle difference to the range [-180, 180]
+        if (angleDifference > 180) {
+            angleDifference -= 360;
+        } else if (angleDifference < -180) {
+            angleDifference += 360;
+        }
+        return calculateRotationSpeedFromRotationAngle(angleDifference);
     }
-    
     // Calculate distance and rotation (angle difference)
     public double[] calculateDistanceAndRotationToHub() {
         Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
@@ -169,18 +226,18 @@ public class DriveControls {
     }
 
     // Calculate rotation speed from rotation angle difference
-    public double calculateRotationSpeedFromRotationAngle(double angleDifference, double autoRotationPower) {
+    public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
         double calDifference = angleDifference;
 
         // Optional: Apply teleop offset if enabled
-        if (Constants.AutoRotation.teleopOffsetEnabled) {
+        if (Constants.AutoPositioning.teleopOffsetEnabled) {
             double teleopOffset = -driver.getRightX();
-            calDifference += teleopOffset * Constants.AutoRotation.teleopOffsetkP;
+            calDifference += teleopOffset * Constants.AutoPositioning.teleopOffsetkP;
         }
 
         // If angle difference is within threshold, don't rotate
         if (Math.abs(calDifference) > 1.0) {
-            return calDifference * autoRotationPower * Constants.AutoRotation.autoRotationToHubkP;
+            return calDifference  * Constants.AutoPositioning.autoRotationkP;
         } else {
             return 0;
         }

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

@@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
+import frc.robot.RobotStatusManager;
 import frc.robot.subsystems.ShooterSubsystem;
 
 public class ShooterControls {
@@ -15,39 +16,64 @@ public class ShooterControls {
     private double flywheelSpeedOffset = 0.0;
     private double backboardPositionOffset = 0.0;
     private NetworkTable shooterControlTable;
-    public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller) {
+    private final RobotStatusManager robotStatusManager;
+    public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) {
         this.shooterSubsystem = shooterSubsystem;
         this.controller = controller;
-
+        this.robotStatusManager = rsm;
         shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl");
     }
 
     public Command defaultShooterCommand() {
         return Commands.run(() -> {
-                double flywheelSpeed;
+            switch (robotStatusManager.getStatus()) {
-                double backboardPosition;
+                case Stopped:
-                double conveyorSpeed;
+                    shooterSubsystem.stopMotors();
-                // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
+                    break;
-                var x = shooterSubsystem.getDistance();
+                case AllTelop:
-                flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
+                case AutoAimming:
-                flywheelSpeed += flywheelSpeedOffset;
+                    double flywheelSpeed = 0;
-                backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684;
+                    double backboardPosition = 0;
-                backboardPosition += backboardPositionOffset;
+                    double conveyorSpeed = 0;
-                conveyorSpeed = Constants.Shooter.conveyorSpeed;
+                    var x = shooterSubsystem.getDistance();
-                shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
+                    flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
-                shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
+                    flywheelSpeed += flywheelSpeedOffset;
-                shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
+                    backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684;
-                // System.out.println("running");
+                    backboardPosition += backboardPositionOffset;
-                if (controller.getRightTriggerAxis() < 0.1) {
+                    conveyorSpeed = Constants.Shooter.conveyorSpeed;
-                    flywheelSpeed = 0;
+                    if (controller.getRightTriggerAxis() < 0.1) {
-                    conveyorSpeed = 0;
+                        flywheelSpeed = 0;
-                }
+                        conveyorSpeed = 0;
+                    }
+                    // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
+                    // double flywheelSpeed = calculateFlywheelSpeed(0);
+                    // double BackboardPosition = calculateBackboardPosition(0);
+                    shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
+                    shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
+                    shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
+                    shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
+                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
+                    shooterSubsystem.setBackboardPosition(backboardPosition);  
+                    if(shooterSubsystem.isBackboardAtTarget()){
+                        shooterSubsystem.backboardMotor.set(0);
+                    }else{
+                        shooterSubsystem.outputBackboard();
+                    }  
+                    break;
+                case Climbing:
+                case CrossingBump:
+                case CrossingTrench:
+                    if(shooterSubsystem.getBackboardPosition() <= 20)
+                        shooterSubsystem.stopMotors();
+                    else{
+                        shooterSubsystem.setBackboardPosition(0);    
+                        shooterSubsystem.outputBackboard();
+                    }
+                    break;
+                    
+            }
+                
 
-                // double flywheelSpeed = calculateFlywheelSpeed(0);
-                // double BackboardPosition = calculateBackboardPosition(0);
-                shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
-                shooterSubsystem.setBackboardPosition(backboardPosition);
         }, shooterSubsystem);
     }
 

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -43,7 +43,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
     private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
     /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
-    private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.kCW_90deg;
+    private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
     /* Keep track if we've ever applied the operator perspective before or not */
     private boolean m_hasAppliedOperatorPerspective = false;
 
@@ -278,6 +278,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
     @Override
     public void periodic() {
+
         Pose2d pose = getState().Pose;
         m_field.setRobotPose(pose);
         posePub.set(pose);

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -33,12 +33,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
     private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
 
-    public enum Mode{
+
-        Inalyzed,
-        UnInalyed,
-        Inalyzing;
-    }
-    public Mode currentBackboardInalyzeMode;
     // 初始化各个部件
     public ShooterSubsystem() {
         // 初始化飞轮电机
@@ -75,7 +70,6 @@ public class ShooterSubsystem extends SubsystemBase {
         
         shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
 
-        currentBackboardInalyzeMode = Mode.UnInalyed;
     }
 
     public void setRightFollowLeft(){
@@ -145,14 +139,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
     @Override
     public void periodic() {
-        if(currentBackboardInalyzeMode != Mode.Inalyzing){
+        backboardPID.calculate(getBackboardPosition());
-            backboardPID.calculate(getBackboardPosition());
-            if(isBackboardAtTarget()){
-                backboardMotor.set(0);
-            }else{
-                outputBackboard();
-            }
-        }
         shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble());
         shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble());
         shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble());
@@ -161,7 +148,6 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
         shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
         shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
-        shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name());
     }
 
     public double getDistance(){