自动对准bump和trench+自动旋转修正版

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

@@ -33,7 +33,7 @@ public final class Constants {
             public static final double kD = 0.0;
             
         }
-        public static final double backboardSpeedMax = 60; //也就是output
+        public static final double backboardSpeedMax = 30; //也就是output
         // 背板的上下限位角度
         public static final double backboardUpLimit = 3000.0;  // 背板上限角度（根据实际需求设置）
         public static final double backboardDownLimit = 0.0;  // 背板下限角度（根据实际需求设置）
@@ -50,9 +50,13 @@ public final class Constants {
         public static final boolean teleopOffsetEnabled = true;
         public static final double teleopOffsetkP = 15;
 
-        public static final double autoPositioningkP = 0.1;
+        public static final double autoPositioningkP = 1.0;
+
         public static final double[] bumpY = {2.51,5.556};
         public static final double[] trenchY = {0.639,7.43};
+
+        public static final double TrenchtargetAngle = 0;
+        public static final double TurningkP = 1.5;
     }
 
     public class StartingPoints{

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

@@ -6,6 +6,7 @@ import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -60,6 +61,13 @@ public class DriveControls {
             .withRotationalDeadband(maxAngularRate * 0.05)
             .withDriveRequestType(DriveRequestType.Velocity);
 
+    private final SwerveRequest.FieldCentric fieldCentricAuto =
+    new SwerveRequest.FieldCentric()
+        .withDeadband(maxSpeed * 0.05)
+        .withRotationalDeadband(maxAngularRate * 0.05)
+        .withDriveRequestType(DriveRequestType.Velocity)
+        .withForwardPerspective(SwerveRequest.ForwardPerspectiveValue.BlueAlliance);
+
     private final SwerveRequest.RobotCentric robotCentric =
     new SwerveRequest.RobotCentric()
         .withDeadband(maxSpeed * 0.05)
@@ -125,14 +133,64 @@ public class DriveControls {
                 break;
             case CrossingBump:
                 double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
-                vy = differenceBump * Constants.AutoPositioning.autoPositioningkP;
+                if (Math.abs(differenceBump) < 0.1) {
+                    vy = 0;
+                } else {
+                    double raw = differenceBump * Constants.AutoPositioning.autoPositioningkP;
+                    vy = Math.copySign(
+                        Math.max(Math.abs(raw), 0.4),
+                        raw
+                    );
+                }
                 autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
+
+                Rotation2d BumpcurrentAngle = drivetrain.getState().Pose.getRotation();
+                double BumpcurrentRad = BumpcurrentAngle.getRadians();
+                double targetA = Math.toRadians(135.0);
+                double targetB = Math.toRadians(-135.0);
+                // 计算误差
+                double errorA = targetA - BumpcurrentRad;
+                double errorB = targetB - BumpcurrentRad;
+                // 角度归一化到 [-pi, pi]
+                errorA = Math.atan2(Math.sin(errorA), Math.cos(errorA));
+                errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
+                // 选误差绝对值更小的那个
+                double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
+
+                if (Math.abs(chosenError) < Math.toRadians(2.0)) {
+                    vomega = 0.0;
+                } else {
+                    vomega = chosenError * Constants.AutoPositioning.TurningkP;
+                }
+                // 限制最大角速度
+                vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
+
                 //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;
+                if (Math.abs(differenceTrench) < 0.1) {
+                    vy = 0;
+                } else {
+                    double raw = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
+                    vy = Math.copySign(
+                        Math.max(Math.abs(raw), 0.4),
+                        raw
+                    );
+                }
                 autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
+                Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
+                double currentRad = currentAngle.getRadians();
+                double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
+                double TrenchAngleerror = targetRad - currentRad;
+                if (Math.abs(TrenchAngleerror) < Math.toRadians(1.0)) {
+                    vomega = 0.0;
+                } else {
+                    vomega = TrenchAngleerror * Constants.AutoPositioning.TurningkP;
+                }
+                // 限幅（防止转太快）
+                vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
+
                 //vomega = calculateRotationSpeedForTrench();
                 break;
         }        
@@ -141,10 +199,15 @@ public class DriveControls {
         autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
         autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
         
-        if (fieldCentricEnabled || 
+        boolean isAutoLike =
             robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench) {
+            robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
-            return fieldCentric
+
+        if (fieldCentricEnabled || isAutoLike) {
+
+    // 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
+        var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
+        return chosen
                 .withVelocityX(vx)
                 .withVelocityY(vy)
                 .withRotationalRate(vomega);
@@ -161,6 +224,7 @@ public class DriveControls {
     }
 
     /**
+     * 
          * 输出移动到距离两个目标之间较近的那个点的位移
          * @return 位移 有正负
          */

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

@@ -36,6 +36,7 @@ public class ShooterControls {
                     double backboardPosition = 0;
                     double conveyorSpeed = 0;
                     var x = shooterSubsystem.getDistance();
+                    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 += 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;