From 1f397e6b6e48a3f7732ea01ba38239506421b162 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Tue, 10 Mar 2026 21:01:01 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B1=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../pathplanner/autos/Auto climb only.auto | 19 +++++++ .../pathplanner/autos/Shoot and Climb.auto | 18 +++++++ .../pathplanner/paths/Climb Step2 Red.path | 10 ++-- .../pathplanner/paths/Climb Step3 Red.path | 12 ++--- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 30 +++++------ .../java/frc/robot/controls/Autocommand.java | 52 +++++++++---------- .../frc/robot/generated/TunerConstants.java | 12 ++++- .../robot/subsystems/ClimberSubsystem.java | 20 +++---- .../subsystems/CommandSwerveDrivetrain.java | 8 +-- 10 files changed, 115 insertions(+), 70 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Auto climb only.auto diff --git a/src/main/deploy/pathplanner/autos/Auto climb only.auto b/src/main/deploy/pathplanner/autos/Auto climb only.auto new file mode 100644 index 0000000..d454b1f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Auto climb only.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "start_intake_timelimit" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Shoot and Climb.auto b/src/main/deploy/pathplanner/autos/Shoot and Climb.auto index 7c9ce90..33b931f 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Climb.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Climb.auto @@ -22,12 +22,30 @@ "pathName": "Shoot to Climb Step1 Red" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "path", "data": { "pathName": "Climb Step2 Red" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "climb_up" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Climb Step2 Red.path b/src/main/deploy/pathplanner/paths/Climb Step2 Red.path index 6e68a52..393510c 100644 --- a/src/main/deploy/pathplanner/paths/Climb Step2 Red.path +++ b/src/main/deploy/pathplanner/paths/Climb Step2 Red.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 15.610835339716738, - "y": 3.3590426144037364 + "x": 15.522355156858092, + "y": 3.365 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 15.467, + "x": 15.517, "y": 3.365 }, "prevControl": { - "x": 15.743555432745389, - "y": 3.3550226257587115 + "x": 15.77785583456356, + "y": 3.365 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Climb Step3 Red.path b/src/main/deploy/pathplanner/paths/Climb Step3 Red.path index bcac179..b84853e 100644 --- a/src/main/deploy/pathplanner/paths/Climb Step3 Red.path +++ b/src/main/deploy/pathplanner/paths/Climb Step3 Red.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 15.467, + "x": 15.517, "y": 3.365 }, "prevControl": null, "nextControl": { - "x": 15.716589593303949, - "y": 3.3506809597259686 + "x": 15.767, + "y": 3.365 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 15.467, + "x": 15.517, "y": 3.465 }, "prevControl": { - "x": 15.191301517038063, - "y": 3.4933990509259307 + "x": 15.239842716853756, + "y": 3.465 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ae377b5..a873160 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,7 +42,7 @@ public final class Constants { public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置） } public class AutoPositioning { - public static final double autoRotationkP = 0.1; + public static final double autoRotationkP = 0.08; public static final double autoRotationForBumpTargetDegrees = 45; public static final double autoRotationForTrenchTargetDegrees = 0; public static final boolean teleopOffsetEnabled = true; @@ -149,7 +149,7 @@ public final class Constants { public static double climbUpTicks = 94; // TODO: 实测后调整 public static double climbDownTicks = 0; // TODO: 实测后调整 - public static double climbDownAutoTicks = -5; // TODO: 实测后调整，自动爬时可能不需要完全降到底 + public static double climbDownAutoTicks = 0; // TODO: 实测后调整，自动爬时可能不需要完全降到底 public static final double climbVelocity = -200; public static final double holdToleranceRot = 0.5; public static final double deltaDownTicks = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4fda7db..ac85e77 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -91,7 +91,7 @@ public class RobotContainer { configueClimber(); bingdingLED(); configueIntaker(); - configureBindings(); + //configureBindings(); } /* ====================== */ @@ -202,21 +202,21 @@ public class RobotContainer { controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem)); } - private void configureBindings() { - teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand()); - } + // private void configureBindings() { + // teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand()); + // } - private Command handleClimberAtTeleopStartCommand() { - return new InstantCommand(() -> { - if (climberSubsystem.wasAutoClimbUsedInAuto()) { - climberSubsystem.climbUp(); - climberSubsystem.clearAutoClimbUsedInAutoFlag(); - } - }, climberSubsystem); - } + // private Command handleClimberAtTeleopStartCommand() { + // return new InstantCommand(() -> { + // if (climberSubsystem.wasAutoClimbUsedInAuto()) { + // climberSubsystem.climbUp(); + // climberSubsystem.clearAutoClimbUsedInAutoFlag(); + // } + // }, climberSubsystem); + // } - public ClimberSubsystem getClimberSubsystem() { - return climberSubsystem; - } + // public ClimberSubsystem getClimberSubsystem() { + // return climberSubsystem; + // } } diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index dd2de02..158b81f 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -32,7 +32,7 @@ import frc.robot.Constants.RobotStatus; ShooterControls shooterControls ) { NamedCommands.registerCommand("climb_up", climb_up(climber)); - NamedCommands.registerCommand("climb_down", climb_down(climber)); + //NamedCommands.registerCommand("climb_down", climb_down(climber)); NamedCommands.registerCommand("start_intake", start_intake(intaker)); NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker)); NamedCommands.registerCommand("stop_intake", stop_intake(intaker)); @@ -40,32 +40,30 @@ import frc.robot.Constants.RobotStatus; NamedCommands.registerCommand("intaker_up", intaker_up(intaker)); NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain)); NamedCommands.registerCommand("shoot", shoot(shooter)); - NamedCommands.registerCommand("Shake", Autocommand.shake(driveControls)); - NamedCommands.registerCommand("AutoAim", Autocommand.autoAim(driveControls)); - NamedCommands.registerCommand("AutoShoot", Autocommand.autoShoot(shooterControls)); + // NamedCommands.registerCommand("Shake", Autocommand.shake(driveControls)); + // NamedCommands.registerCommand("AutoAim", Autocommand.autoAim(driveControls)); + // NamedCommands.registerCommand("AutoShoot", Autocommand.autoShoot(shooterControls)); } public static Command climb_up(ClimberSubsystem climber) { return new RunCommand(() -> { climber.climbUp(); // 控制爬升器上升 - }, climber).withTimeout(1.0).andThen(new RunCommand(() -> { - climber.climbUpNonStopforAuto(); - }, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> { + }, climber).withTimeout(1.0).andThen(new InstantCommand(() -> { climber.holdPosition(); // 停止并锁定当前爬升器位置 }, climber)); } - public static Command climb_down(ClimberSubsystem climber) { - return Commands.sequence( - new InstantCommand(climber::markAutoClimbUsedInAuto), - new RunCommand(() -> { - climber.climbDownAuto(); - }, climber).withTimeout(2.0), - new InstantCommand(() -> { - climber.holdPosition(); - }, climber) - ); - } + // public static Command climb_down(ClimberSubsystem climber) { + // return Commands.sequence( + // new InstantCommand(climber::markAutoClimbUsedInAuto), + // new RunCommand(() -> { + // climber.climbDownAuto(); + // }, climber).withTimeout(2.0), + // new InstantCommand(() -> { + // climber.holdPosition(); + // }, climber) + // ); + // } public static Command start_intake(IntakerSubsystem intaker) { return new RunCommand(() -> { @@ -138,15 +136,15 @@ import frc.robot.Constants.RobotStatus; shooter.setConveyorSpeedByRPS(0); // 停止输送 }, shooter))); } - public static Command autoAim(DriveControls driveControls) { - return driveControls.autoAimCommand(); - } - public static Command autoShoot(ShooterControls shooterControl) { - return shooterControl.autoShootToHubCommand(); - } - public static Command shake(DriveControls driveControls) { - return driveControls.shakeCommand(); - } + // public static Command autoAim(DriveControls driveControls) { + // return driveControls.autoAimCommand(); + // } + // public static Command autoShoot(ShooterControls shooterControl) { + // return shooterControl.autoShootToHubCommand(); + // } + // public static Command shake(DriveControls driveControls) { + // return driveControls.shakeCommand(); + // } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 05b7dfc..dcb6c54 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -66,7 +66,17 @@ public class TunerConstants { ); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; + private static final Pigeon2Configuration pigeonConfigs = + new Pigeon2Configuration() + .withGyroTrim( + new GyroTrimConfigs() + .withGyroScalarZ(-5.0) + ) + .withPigeon2Features( + new Pigeon2FeaturesConfigs() + .withEnableCompass(false) + ); + // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index e01dd1a..954eeab 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -16,7 +16,7 @@ import frc.robot.Constants; public class ClimberSubsystem extends SubsystemBase { - private boolean autoClimbUsedInAuto = false; + // private boolean autoClimbUsedInAuto = false; private final TalonFX climberMotor; private double targetPositionTicks; @@ -102,17 +102,17 @@ public class ClimberSubsystem extends SubsystemBase { climberMotor.set(0); } - public void markAutoClimbUsedInAuto() { - autoClimbUsedInAuto = true; - } + // public void markAutoClimbUsedInAuto() { + // autoClimbUsedInAuto = true; + // } - public boolean wasAutoClimbUsedInAuto() { - return autoClimbUsedInAuto; - } + // public boolean wasAutoClimbUsedInAuto() { + // return autoClimbUsedInAuto; + // } - public void clearAutoClimbUsedInAutoFlag() { - autoClimbUsedInAuto = false; - } + // public void clearAutoClimbUsedInAutoFlag() { + // autoClimbUsedInAuto = false; + // } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index a70c641..f835e94 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -163,7 +163,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su public void forceUsingLimelightmt2() { LimelightHelpers.PoseEstimate mt2 = - LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Shooter); + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter); if (mt2 == null || mt2.tagCount == 0) { return; @@ -258,7 +258,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private LimelightHelpers.PoseEstimate getLimelightPoseEstimate(String limelightName) { if (Constants.Limelight.UsingMetaTag2) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); } return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); } @@ -415,8 +415,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su Pose2d pose = getState().Pose; m_field.setRobotPose(pose); posePub.set(pose); - shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose); - intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose); + shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose); + intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose); driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees()); driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name()); driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);