diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 519e5a6..b12add4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,6 +78,8 @@ public final class Constants { public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter"; public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker"; public static final Boolean UsingMetaTag2 = true; + public static final int climbPipelineIndex = 8; + public static final int locatePipelineIndex = 0; public static final double AutoClimbAlignSpeed = 0.5;//等查资料 public static final double AutoClimbAlignTolerance = 0.1;//？ public static final double AutoClimbmaxAngularVelocity = 0.5;//等查资料 diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index c008e1b..22ad966 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -1,5 +1,6 @@ package frc.robot.controls; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; @@ -11,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.RobotStatusManager; import frc.robot.subsystems.IntakerSubsystem; +import frc.robot.subsystems.LimelightSupplier; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.Constants; @@ -58,7 +60,25 @@ import frc.robot.Constants.RobotStatus; intaker.stopIntakerotater();; // 停止旋转 }, intaker)); } + public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) { + return Commands.sequence( + new InstantCommand(() -> { + LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); + }), + Commands.runEnd( + () -> { + drivetrain.driveToAprilTag(); + }, + () -> { + LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); + }, + drivetrain + ).until(() -> drivetrain.isAlignedToAprilTag()) + ); + } + + } diff --git a/src/main/java/frc/robot/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java index 6ee58ab..570d323 100644 --- a/src/main/java/frc/robot/controls/ClimberControls.java +++ b/src/main/java/frc/robot/controls/ClimberControls.java @@ -1,11 +1,16 @@ package frc.robot.controls; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.LimelightSupplier; public class ClimberControls { @@ -53,4 +58,7 @@ public class ClimberControls { public Command resetClimberEncoderCommand(ClimberSubsystem climber) { return Commands.runOnce(climber::zeroEncoder, climber); } + + + } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 0bbb085..588c54b 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -20,6 +20,7 @@ import frc.robot.Constants.DriveMode; import frc.robot.Constants.RobotStatus; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.LimelightSupplier; import java.util.Optional; import edu.wpi.first.wpilibj.DriverStation; @@ -145,7 +146,7 @@ public class DriveControls { autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); break; case Climbing: - drivetrain.driveToAprilTag(); + alignToClimb(); break; case CrossingBump: double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]); @@ -397,6 +398,17 @@ public class DriveControls { } public Command getAllianceColorCommand() { return Commands.runOnce(this::setTeamColors); -} + } + public void alignToClimb(){ + LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); + if(!drivetrain.isAlignedToAprilTag()){ + drivetrain.driveToAprilTag(); + } + else{ + LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); + drivetrain.stop(); + } + } + } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index b839316..92432e3 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -47,6 +47,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; + private boolean isAlignedToAprilTag = false; private DriveMode m_driveMode = DriveMode.OTHER; @@ -64,6 +65,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private Field2d shooter_ll_field; private Field2d intaker_ll_field; + public CommandSwerveDrivetrain( SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules @@ -130,6 +132,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled) ); } + + public void stop() { + setControl( + new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 0)) + ); + } private static final double kMaxVisionJumpMeters = 1.0; private static final double kMaxOmegaDegPerSec = 360.0; @@ -324,8 +332,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su ry = Math.min(ry, Constants.Limelight.AutoClimbmaxAngularVelocity); ry = Math.max(ry, -Constants.Limelight.AutoClimbmaxAngularVelocity); } - - + isAlignedToAprilTag = Math.abs(tx) < Constants.Limelight.AutoClimbAlignTolerance && Math.abs(targetTz) < Constants.Limelight.AutoClimbAlignTolerance && Math.abs(ry) < Constants.Limelight.AutoClimbAlignTolerance; ChassisSpeeds scaled = new ChassisSpeeds( tx, // X 不变 targetTz, // Y 不变 @@ -341,6 +348,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return "Driving to AprilTag"; } + public boolean isAlignedToAprilTag(){ + return isAlignedToAprilTag; + } @Override public void periodic() { @@ -352,7 +362,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees()); driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name()); driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC); - + positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag()); + positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX()); + positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY()); + positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ()); + positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false); // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0 LimelightHelpers.SetRobotOrientation( Constants.Limelight.LIMELIGHT_NAME_Shooter,