diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e2982bf..9f43dac 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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{ diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index db2d524..62fd356 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/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; } @@ -140,11 +198,16 @@ public class DriveControls { autoControlNetworkTable.getEntry("ActualVX").setDouble(vx); autoControlNetworkTable.getEntry("ActualVY").setDouble(vy); autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega); + + boolean isAutoLike = + robotStatusManager.getStatus() == RobotStatus.CrossingBump || + robotStatusManager.getStatus() == RobotStatus.CrossingTrench; - if (fieldCentricEnabled || - robotStatusManager.getStatus() == RobotStatus.CrossingBump || - 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 位移 有正负 */ diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index 42397a5..cd48a33 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/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;