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/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 37fdd72..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; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9fcb052..9e32324 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,6 +3,8 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f52967c..0cecc7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,9 +70,19 @@ public class RobotContainer { private final SendableChooser autoChooser; private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled); + private final Trigger teleopStartTrigger = new Trigger(DriverStation::isTeleopEnabled); public RobotContainer() { + + Pose2d startingPose = Constants.StartingPoints.Red.Point2; + drivetrain.resetPose(startingPose); + configueSwerve(); + configueShooter(); + configueClimber(); + bingdingLED(); + configueIntaker(); + //configureBindings(); Autocommand.preNameCommands( climberSubsystem, IntakerSubsystem, @@ -81,15 +91,9 @@ public class RobotContainer { driveControls, shooterControls ); - SignalLogger.enableAutoLogging(false); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - configueSwerve(); - configueShooter(); - configueClimber(); - bingdingLED(); - configueIntaker(); } /* ====================== */ @@ -200,4 +204,21 @@ public class RobotContainer { controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem)); } + // private void configureBindings() { + // teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand()); + // } + + // private Command handleClimberAtTeleopStartCommand() { + // return new InstantCommand(() -> { + // if (climberSubsystem.wasAutoClimbUsedInAuto()) { + // climberSubsystem.climbUp(); + // climberSubsystem.clearAutoClimbUsedInAutoFlag(); + // } + // }, 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 d67a156..05528ac 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,9 +40,9 @@ 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)); } @@ -136,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/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java index 570d323..2d3f6aa 100644 --- a/src/main/java/frc/robot/controls/ClimberControls.java +++ b/src/main/java/frc/robot/controls/ClimberControls.java @@ -58,7 +58,5 @@ public class ClimberControls { public Command resetClimberEncoderCommand(ClimberSubsystem climber) { return Commands.runOnce(climber::zeroEncoder, climber); } - - } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 669a15a..954eeab 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -16,6 +16,8 @@ import frc.robot.Constants; public class ClimberSubsystem extends SubsystemBase { + // private boolean autoClimbUsedInAuto = false; + private final TalonFX climberMotor; private double targetPositionTicks; @@ -100,6 +102,18 @@ public class ClimberSubsystem extends SubsystemBase { climberMotor.set(0); } + // public void markAutoClimbUsedInAuto() { + // autoClimbUsedInAuto = true; + // } + + // public boolean wasAutoClimbUsedInAuto() { + // return autoClimbUsedInAuto; + // } + + // public void clearAutoClimbUsedInAutoFlag() { + // autoClimbUsedInAuto = false; + // } + @Override public void periodic() { climberTable.getEntry("velocityRps").setDouble(getVelocityRps()); 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);