修复已知问题

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

@@ -137,8 +137,7 @@ public final class Constants {
     // 单位RPS
     public static final double positionToleranceRotations = 0.00;
     //rotater容差
-    public static final double IntakeGetterK = 0.3; // intake getter速度与触发器输入的比例
+
-    public static final double Getterminspeed = 0.3; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转）
     }   
     public static class Climber {
 

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

@@ -77,7 +77,7 @@ public class IntakerControls {
         double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
         
         if (controller.getLeftTriggerAxis() > 0.1) {
-            getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed;
+            getterspeed = controller.getLeftTriggerAxis() * 0.3 + 0.3;
             ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake);
             }
 
@@ -85,8 +85,10 @@ public class IntakerControls {
 
         if(controller.getLeftY() < -0.5){
                     IntakerSubsystem.IntakerUpNonStop();
-            } else if(controller.getLeftY() > 0.5){
+            } else if(controller.getLeftY() > 0.2){
-                    IntakerSubsystem.IntakerDownNonStop();
+                    double rotaterspeed = controller.getLeftY() * 0.15 + 0.05;
+                    IntakerSubsystem.setIntakerRotater(rotaterspeed);
+
             } else {
                     IntakerSubsystem.stopIntakerotater();
             }

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

@@ -39,11 +39,11 @@ public class ShooterControls {
                 case AllTelop:
                 case PassingBall:
                     var xP = shooterSubsystem.getDistanceToPassball();
-                    //xP = Math.max(1.2, Math.min(4.5, xP));
+                    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 += flywheelSpeedOffset;
                     backboardPosition = xP*xP;//todo:实测传球的时候的背板位置 打表
-                    backboardPosition += backboardPositionOffset;
+                    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;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {
                         flywheelSpeed = 0;

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

@@ -353,33 +353,44 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     // 2) 旋转控制：tx 为主，ry 为辅
     //    tx 负责“镜头中心对准 tag”
     //    ry 负责“机器人姿态微调”
-    double omegaFromTx = 0.0;
+    // double omegaFromTx = 0.0;
-    double omegaFromRy = 0.0;
+    // double omegaFromRy = 0.0;
 
-    if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
+    // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
-        omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
+    //     omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
-    }
+    // }
 
-    if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
+    // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
-        omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
+    //     omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
-    }
+    // }
 
-    omega = omegaFromTx + omegaFromRy;
+    // omega = omegaFromTx + omegaFromRy;
 
-    // 大偏角时直接给较明显的转速，先把朝向拉回来
+    // // 大偏角时直接给较明显的转速，先把朝向拉回来
-    if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
+    // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
-        omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
+    //     omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
-    }
+    // }
 
-    // 最小输出，避免转不动
+    // // 最小输出，避免转不动
-    if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
+    // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
-        omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
+    //     omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
-    }
+    // }
+
+    // // 最大输出限幅
+    // omega = clamp(omega,
+    //     -Constants.Limelight.AutoClimbMaxOmega,
+    //      Constants.Limelight.AutoClimbMaxOmega);
+
+    Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
+    double climbCurrentRad = ClimbcurrentAngle.getRadians();
+    double TargetAngleforClimbing = Math.toRadians(0);
+    double Climberror = TargetAngleforClimbing - climbCurrentRad;
+    if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
+            omega = 0.0;
+        } else {
+            omega = Climberror * Constants.AutoPositioning.TurningkP;
+        }
 
-    // 最大输出限幅
-    omega = clamp(omega,
-        -Constants.Limelight.AutoClimbMaxOmega,
-         Constants.Limelight.AutoClimbMaxOmega);
 
     // ===== 调试输出 =====
     driveNetworkTable.getEntry("ClimbTX").setDouble(tx);

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

@@ -101,6 +101,9 @@ public class IntakerSubsystem extends SubsystemBase{
         } else {
             // at or above upper limit: stop
             stopIntakerotater();
+            }
         }
+    public void setIntakerRotater(double rotaterspeed){
+        intakerotater.set(rotaterspeed);
     }
 }