手动修复已知问题 自动第一版

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

@@ -1,5 +1,7 @@
 package frc.robot;
 
+import javax.net.ssl.TrustManager;
+
 import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.geometry.Pose2d;
 
@@ -77,7 +79,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;
+        public static final Boolean UsingMetaTag2 = true; // 是否使用 MetaTag2（如果你们的 pipeline 配置了这个功能的话）
         public static final int climbPipelineIndex = 8;
         public static final int locatePipelineIndex = 0;
 

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

@@ -117,19 +117,19 @@ public class RobotContainer {
     controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
     
     // Start：强行使用metaTag2全场定位
-    controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
+    controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
 
     //LM 按下时autoaimming，松开时alltelop
     // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
     // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
 
     //双操均可触发autoaimming模式，松开时alltelop
-    Trigger autoAim = controllerupper.rightBumper().or(controllerlower.rightStick());
+    Trigger autoAim = controllerupper.rightBumper();//.or(controllerlower.rightStick());
     autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
     autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
 
     //双操均可触发passingball模式，松开时alltelop
-    Trigger autoAim2 = controllerupper.leftBumper().or(controllerlower.leftStick());
+    Trigger autoAim2 = controllerupper.leftBumper();//.or(controllerlower.leftStick());
     autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
     autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
 

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

@@ -1,5 +1,7 @@
 package frc.robot.controls;
 
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -19,7 +21,23 @@ import frc.robot.Constants;
 import frc.robot.Constants.RobotStatus;
 
     public class Autocommand extends SequentialCommandGroup{
-        
+
+        public static void preNameCommands(
+            ClimberSubsystem climber,
+            IntakerSubsystem intaker,
+            CommandSwerveDrivetrain drivetrain,
+            ShooterSubsystem shooter
+    ) {
+        NamedCommands.registerCommand("climb_up", climb_up(climber));
+        NamedCommands.registerCommand("climb_down", climb_down(climber));
+        NamedCommands.registerCommand("start_intake", start_intake(intaker));
+        NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
+        NamedCommands.registerCommand("intaker_down", intaker_down(intaker));
+        NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
+        NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
+        NamedCommands.registerCommand("shoot", shoot(shooter));
+    }
+
         public static Command climb_up(ClimberSubsystem climber) {
             return new RunCommand(() -> {
                 climber.climbUp();  // 控制爬升器上升
@@ -73,12 +91,23 @@ import frc.robot.Constants.RobotStatus;
                     LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
                 },
                 drivetrain
-            ).until(() -> drivetrain.isAlignedToAprilTag())
+            ).until(() -> drivetrain.isAlignedToAprilTag()));
-        );
+     }
-    }
+
+        public static Command shoot(ShooterSubsystem shooter) {
+            return new InstantCommand(() -> {
+                shooter.setFlywheelSpeedByRPS(60);
+                shooter.setBackboardPosition(200);  // 设置自动射击转速
+            }, shooter).withTimeout(1.0).andThen(new InstantCommand(() -> {
+                shooter.setConveyorSpeedByRPS(25);
+            }, shooter).withTimeout(5.0).andThen(new InstantCommand(() -> {
+                shooter.setFlywheelSpeedByRPS(0);  // 停止射击
+                shooter.setBackboardPosition(0);  // 恢复背板位置
+                shooter.setConveyorSpeedByRPS(0);  // 停止输送
+            }, shooter)));
     
 
-
+        }
     }
 
 

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

@@ -85,9 +85,10 @@ public class IntakerControls {
 
         if(controller.getLeftY() < -0.5){
                     IntakerSubsystem.IntakerUpNonStop();
-            } else if(controller.getLeftY() > 0.2){
+            } else if(controller.getLeftY() > 0.5){
-                    double rotaterspeed = controller.getLeftY() * 0.15 + 0.05;
+                    IntakerSubsystem.IntakerDownNonStop();
-                    IntakerSubsystem.setIntakerRotater(rotaterspeed);
+                    // double rotaterspeed = -controller.getLeftY() * 0.3 + 0.2;
+                    // IntakerSubsystem.setIntakerRotater(rotaterspeed);
 
             } else {
                     IntakerSubsystem.stopIntakerotater();

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

@@ -40,16 +40,16 @@ public class ShooterControls {
                 case PassingBall:
                     var xP = shooterSubsystem.getDistanceToPassball();
                     xP = Math.max(1.2, Math.min(4.5, xP));
-                    flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351;
+                    flywheelSpeed = -1.4964026859575532*xP*xP*xP*xP*xP*xP*xP+29.82445640241506*xP*xP*xP*xP*xP*xP-247.71603153159896*xP*xP*xP*xP*xP+1108.8727302184625*xP*xP*xP*xP-2881.517500617481*xP*xP*xP+4332.05363145275*xP*xP-3467.2508337871454*xP+1192.1508062663759;
                     flywheelSpeed += flywheelSpeedOffset;
-                    backboardPosition = xP*xP;//todo:实测传球的时候的背板位置 打表
+                    backboardPosition = +34.5286998307899*xP*xP*xP*xP*xP*xP*xP*xP-788.5788623683884*xP*xP*xP*xP*xP*xP*xP+7658.073303693148*xP*xP*xP*xP*xP*xP-41191.69338972519*xP*xP*xP*xP*xP+133870.54588688645*xP*xP*xP*xP-268559.6987062158*xP*xP*xP+324254.6530324262*xP*xP-215015.48893131423*xP+59977.46545382827;
-                    backboardPosition = -122.11889023593778*xP*xP*xP*xP*xP*xP*xP+2453.385583841308*xP*xP*xP*xP*xP*xP-20553.05413872581*xP*xP*xP*xP*xP+92963.33175750206*xP*xP*xP*xP-244905.25916147238*xP*xP*xP+375150.1363997868*xP*xP-308031.51715612336*xP+104203.52690842684;
+                    backboardPosition += backboardPositionOffset;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {
                         flywheelSpeed = 0;
                         conveyorSpeed = 0;
                     }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.95){
+                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
                         conveyorSpeed = 0;
                     }
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
@@ -67,23 +67,23 @@ public class ShooterControls {
                 case AutoAimming:
                     var x = shooterSubsystem.getDistanceToHub();
                     x = Math.max(1.2, Math.min(4.5, x));
-                    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;
+                    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 += flywheelSpeedOffset;
-                    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;
+                    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 += backboardPositionOffset;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {
                         flywheelSpeed = 0;
                         conveyorSpeed = 0;
                     }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.95){
+                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
                         conveyorSpeed = 0;
                     }
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
                     shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                    shooterSubsystem.setConveyorSpeedOpen(5);
+                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
                     shooterSubsystem.setBackboardPosition(backboardPosition);  
                     if(shooterSubsystem.isBackboardAtTarget()){
                         shooterSubsystem.backboardMotor.set(0);

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

@@ -78,8 +78,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         SmartDashboard.putData("Field", m_field);
         SmartDashboard.putData("PositioningShooterll", shooter_ll_field);
         SmartDashboard.putData("PositioningIntakerll", intaker_ll_field);
-        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 1);
+        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 0);
-        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 1);
+        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 0);
         positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning");
         driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive");
         ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-shooter");
@@ -161,7 +161,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         });
     }
     
-    public void forceUsingLimelightMT2() {
+    public void forceUsingLimelightmt2() {
         LimelightHelpers.PoseEstimate mt2 =
         LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
 
@@ -173,7 +173,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
     }
     
-    private void fuseLimelightMT2() {
+    private void fuseLimelightmt2() {
         Pose2d current = getState().Pose;
 
         LimelightHelpers.PoseEstimate mt2_shooter =
@@ -437,7 +437,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
             0, 0, 0, 0, 0
         );
 
-        fuseLimelightMT2();
+        fuseLimelightmt2();
 
         if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
             DriverStation.getAlliance().ifPresent(allianceColor -> {

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

@@ -97,7 +97,7 @@ public class IntakerSubsystem extends SubsystemBase{
         double currentPosition = getIntakerotaterPosition();
         if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
             // move up while below the upper limit
-            intakerotater.set(0.1);
+            intakerotater.set(0.2);
         } else {
             // at or above upper limit: stop
             stopIntakerotater();