diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c906349..2e90fcb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,35 +15,30 @@ public final class Constants { CrossingTrench } public class Shooter { - public static final double conveyorSpeed = 20; + public static final double conveyorSpeed =25; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) - .withKV(0.0088).withKA(0.0).withKS(0.0); + .withKV(0.099).withKA(0.0).withKS(0.0); public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) - .withKV(0.02).withKA(0.15).withKS(0); + .withKV(0.16).withKA(0.15).withKS(0); public static final Slot0Configs backboardSlot0Configs = new Slot0Configs() .withKP(0.01).withKI(0).withKD(0) - .withKV(0.008).withKA(0.0).withKS(0.010); + .withKV(0.03).withKA(0.0).withKS(0.010); public class backboardPositionPID { - public static final double kP = 0.1; + public static final double kP = 0.3; public static final double kI = 0.0; public static final double kD = 0.0; } - public static final double backboardSpeedMax = 30; //也就是output + public static final double backboardSpeedMax = 60; //也就是output // 背板的上下限位角度 public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置） public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置） } - public class ShooterCalculation{ - public static final double positionNear = 0; // 近距离背板角度 - public static final double positionMid = 1600; // 中距离背板角度 - public static final double postionFar = 3200; // 远距离背板角度 - } public class AutoPositioning { public static final double autoRotationkP = 0.1; public static final double autoRotationForBumpTargetDegrees = 45; @@ -83,14 +78,41 @@ 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 AutoClimbToleranceTX = 1.5; // deg + public static final double AutoClimbToleranceTZ = 0.10; // m + public static final double AutoClimbToleranceRY = 2.0; // deg + + // P 控制系数 + public static final double AutoClimbKpTX = 0.020; // deg -> rad/s + public static final double AutoClimbKpTZ = 0.90; // m -> m/s + public static final double AutoClimbKpRY = 0.015; // deg -> rad/s + + // 最小输出（防止快到目标时推不动） + public static final double AutoClimbMinVX = 0.18; // m/s + public static final double AutoClimbMinOmega = 0.10; // rad/s + + // 最大输出（防止冲太猛） + public static final double AutoClimbMaxVX = 1.20; // m/s + public static final double AutoClimbMaxOmega = 0.60; // rad/s + + // 远距离 / 大角度时额外限速，可后续微调 + public static final double AutoClimbFastTurnThresholdTX = 15.0; // deg + public static final double AutoClimbFastTurnOmega = 0.45; // rad/s } public class Field { public static final double RedHubPositionX = 11.914; public static final double RedHubPositionY = 4.034; - public static final double PassingBallPosX = 0; - public static final double PassingBallPosY1 = 2.51; - public static final double PassingBallPosY2 = 5.556; + public static final double BlueHubPositionX = 4.625; + public static final double BlueHubPositionY = 4.034; + public static final double PassingBallPosY1 = 1.405; + public static final double PassingBallPosY2 = 6.664; + public static final double RedPassingBallPosX = 13.355; + public static final double BluePassingBallPosX = 3.184; } @@ -111,12 +133,12 @@ public final class Constants { .withKV(0.00).withKA(0.00).withKS(0.00); //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI) - public static final double IntakeGetterSpeedforAuto = 0.0; + public static final double IntakeGetterSpeedforAuto = 0.4; // 单位RPS public static final double positionToleranceRotations = 0.00; //rotater容差 public static final double IntakeGetterK = 0.3; // intake getter速度与触发器输入的比例 - public static final double Getterminspeed = 0.2; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转） + public static final double Getterminspeed = 0.3; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转） } public static class Climber { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 510bfba..c0a8a3d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -58,7 +59,7 @@ public class RobotContainer { private final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager ); private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager); private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper); - private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, controllerupper); + private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper); private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain); private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs); private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs); @@ -118,9 +119,19 @@ public class RobotContainer { // Start：强行使用metaTag2全场定位 controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2)); - //LM 按下时autoaimming - controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); - controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + //LM 按下时autoaimming，松开时alltelop + // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); + // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + + //双操均可触发autoaimming模式，松开时alltelop + Trigger autoAim = controllerupper.rightBumper().or(controllerlower.rightStick()); + autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); + autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + + //双操均可触发passingball模式，松开时alltelop + Trigger autoAim2 = controllerupper.leftBumper().or(controllerlower.leftStick()); + autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall)); + autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); controllerlower.leftTrigger() .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) @@ -129,6 +140,10 @@ public class RobotContainer { controllerlower.rightTrigger() .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + + controllerlower.a() + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing)) + .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); } private void configueShooter(){ diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index c008e1b..a1d9342 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 c16afb7..a9cb59b 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -19,12 +19,14 @@ 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; import edu.wpi.first.wpilibj.DriverStation.Alliance; public class DriveControls { + public boolean isRedAlliance; private final CommandSwerveDrivetrain drivetrain; @@ -122,7 +124,9 @@ public class DriveControls { // 将距离值写入 NetworkTable autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]); autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); - + autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]); + autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]); + switch (robotStatusManager.getStatus()) { case Stopped: vx = 0; @@ -130,10 +134,12 @@ public class DriveControls { vomega = 0; break; case PassingBall: + LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); break; case AutoAimming: + LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); break; @@ -141,6 +147,7 @@ public class DriveControls { autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); break; case Climbing: + alignToClimb(); break; case CrossingBump: double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]); @@ -152,6 +159,7 @@ public class DriveControls { Math.max(Math.abs(raw), 0.4), raw ); + vy = isRedAlliance ? -vy : vy; } autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump); @@ -181,6 +189,7 @@ public class DriveControls { Math.max(Math.abs(raw), 0.4), raw ); + vy = isRedAlliance ? -vy : vy; } autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench); Rotation2d currentAngle = drivetrain.getState().Pose.getRotation(); @@ -204,7 +213,8 @@ public class DriveControls { if (fieldCentricEnabled || isAutoLike) { // 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective） - var chosen = isAutoLike ? fieldCentricAuto : fieldCentric; + //var chosen = isAutoLike ? fieldCentricAuto : fieldCentric; + var chosen = fieldCentric; return chosen .withVelocityX(vx) .withVelocityY(vy) @@ -233,8 +243,8 @@ public class DriveControls { } public double[] calculateDistanceAndRotationToHub() { Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 - double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标 - double targetY = Constants.Field.RedHubPositionY; // 目标 Y 坐标 + double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX; + double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY; // 计算目标角度（相对于场地坐标系） double deltaX = targetX - currentPose.getX(); @@ -261,7 +271,7 @@ public class DriveControls { } public double[] calculateDistanceAndRotationToPassBall() { Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 - double targetX = Constants.Field.PassingBallPosX; // 目标 X 坐标 + double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 目标 X 坐标 double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标 double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标 @@ -389,6 +399,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/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index e63b99d..e456c87 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -6,16 +6,19 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.IntakerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.Constants; public class IntakerControls { private final IntakerSubsystem IntakerSubsystem; private final CommandXboxController controller; + private final ShooterSubsystem ShooterSubsystem; //private static final double ROTATER_STEP_PER_CYCLE = 0.00; - public IntakerControls(IntakerSubsystem Intaker, CommandXboxController controller) { + public IntakerControls(IntakerSubsystem Intaker, ShooterSubsystem Shooter, CommandXboxController controller) { + this.ShooterSubsystem = Shooter; this.IntakerSubsystem = Intaker; this.controller = controller; } @@ -69,10 +72,15 @@ public class IntakerControls { public Command defaultIntakerCommand() { return Commands.run(() -> { - double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; - if (controller.getLeftTriggerAxis() < 0.1) { - getterspeed = 0; + double getterspeed = 0; + double conveyoyspeedforintake = 15; + double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent(); + + if (controller.getLeftTriggerAxis() > 0.1) { + getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; + ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake); } + IntakerSubsystem.setIntakerGetterSpeed(getterspeed); if(controller.getLeftY() < -0.5){ diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index 4d2ae42..cbfc554 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.RobotStatusManager; import frc.robot.Constants.Intaker; +import frc.robot.Constants.RobotStatus; import frc.robot.subsystems.IntakerSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -37,7 +38,7 @@ public class ShooterControls { break; case AllTelop: case PassingBall: - var xP = shooterSubsystem.getDistance(); + var xP = shooterSubsystem.getDistanceToPassball(); //xP = Math.max(1.2, Math.min(4.5, xP)); flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351; flywheelSpeed += flywheelSpeedOffset; @@ -48,12 +49,9 @@ public class ShooterControls { flywheelSpeed = 0; conveyorSpeed = 0; } - if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()... modules @@ -127,12 +131,15 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su setControl( 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; @@ -263,19 +270,134 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su // 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿） double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号 if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) { - System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed"); + //System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed"); return false; } // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略） //Pose2d current = getState().Pose; if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) { - System.out.println("NoUsingMetaTag2ForTooChangingPose"); + //System.out.println("NoUsingMetaTag2ForTooChangingPose"); return false; } return true; } + private double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } + + public String driveToAprilTag() { + System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)); + System.out.println("TV = " + LimelightSupplier.isTargetVisible()); + + if (!LimelightSupplier.isTargetVisible()) { + isAlignedToAprilTag = false; + //stop(); + return "No Target"; + } + + RawFiducial[] fiducials = + LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); + + boolean foundDesiredTag = false; + for (RawFiducial fiducial : fiducials) { + if (fiducial.id == 16 || fiducial.id == 32) { + foundDesiredTag = true; + break; + } + } + + if (!foundDesiredTag) { + isAlignedToAprilTag = false; + stop(); + return "No Desired Tag"; + } + + // ===== 读取原始误差 ===== + double tx = LimelightSupplier.getTX(); // deg + double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号 + double ry = LimelightSupplier.getTargetRotationY(); // deg + + // ===== 用原始误差判断是否已对齐 ===== + isAlignedToAprilTag = + Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX && + Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ && + Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY; + + if (isAlignedToAprilTag) { + stop(); + return "Aligned"; + } + + // ===== 计算输出 ===== + double vx = 0.0; + double vy = 0.0; + double omega = 0.0; + + // 1) 前后控制：根据 tz 前进/后退 + if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) { + vx = tz * Constants.Limelight.AutoClimbKpTZ; + + // 最小输出，避免快到目标时不动 + if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) { + vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx); + } + + // 最大输出限幅 + vx = clamp(vx, + -Constants.Limelight.AutoClimbMaxVX, + Constants.Limelight.AutoClimbMaxVX); + } + + // 2) 旋转控制：tx 为主，ry 为辅 + // tx 负责“镜头中心对准 tag” + // ry 负责“机器人姿态微调” + double omegaFromTx = 0.0; + double omegaFromRy = 0.0; + + if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) { + omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX; + } + + if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) { + omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY; + } + + omega = omegaFromTx + omegaFromRy; + + // 大偏角时直接给较明显的转速，先把朝向拉回来 + if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) { + omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega); + } + + // 最小输出，避免转不动 + if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) { + omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega); + } + + // 最大输出限幅 + omega = clamp(omega, + -Constants.Limelight.AutoClimbMaxOmega, + Constants.Limelight.AutoClimbMaxOmega); + + // ===== 调试输出 ===== + driveNetworkTable.getEntry("ClimbTX").setDouble(tx); + driveNetworkTable.getEntry("ClimbTZ").setDouble(tz); + driveNetworkTable.getEntry("ClimbRY").setDouble(ry); + driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx); + driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega); + driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag); + + // ===== 下发到底盘 ===== + driveRobotRelative(new ChassisSpeeds(vx, vy, omega)); + + return "Driving to AprilTag"; +} + + public boolean isAlignedToAprilTag(){ + return isAlignedToAprilTag; + } @Override public void periodic() { @@ -287,7 +409,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, diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index 599d16f..61f4299 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -83,26 +83,9 @@ public class IntakerSubsystem extends SubsystemBase{ stopIntakerotater(); } public void IntakerDownNonStop(){ - double currentPosition = getIntakerotaterPosition(); - if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) { - - intakerotater.set(-0.05);} - // double currentPosition = getIntakerotaterPosition(); - // if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) { - // intakerotater.setControl(velocityRequest.withVelocity(Constants.Intaker.intakeRotaterVelocity)); - // } else { - // stopIntakerotater(); - // } - } + intakerotater.set(-0.05); + } public void IntakerUpNonStop(){ - double currentPosition = getIntakerotaterPosition(); - if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) { - intakerotater.set(0.2);} - - // if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) { - // intakerotater.setControl(velocityRequest.withVelocity(-Constants.Intaker.intakeRotaterVelocity)); - // } else { - // stopIntakerotater(); - // } + intakerotater.set(0.1); } } diff --git a/src/main/java/frc/robot/subsystems/LimelightSupplier.java b/src/main/java/frc/robot/subsystems/LimelightSupplier.java new file mode 100644 index 0000000..7836b78 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightSupplier.java @@ -0,0 +1,108 @@ +package frc.robot.subsystems; + +import frc.robot.Constants; + +import java.lang.annotation.Target; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.LimelightHelpers; +import frc.robot.subsystems.LimelightHelpers.RawFiducial; + +public class LimelightSupplier extends SubsystemBase { + + public LimelightSupplier() { + setPipeline(8); + } + + public static double getTX() { + return LimelightHelpers.getTX(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static double getTY() { + return LimelightHelpers.getTY(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static double getTA() { + // Logic to get area from Limelight + return LimelightHelpers.getTA(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static boolean isTargetVisible() { + if(!LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker)){ + return false; + } + // Logic to check if a target is visible + return LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + public static int getAprilTagID(){ + RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); + int id = -1; + for (RawFiducial fiducial : fiducials) { + id = fiducial.id; // 标签ID + } + return id; + } + public static void setPipeline(int pipeline) { + if(((int)LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)) != pipeline){ + LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker, pipeline); + } + } + + public static double[] getTargetPose() { + // Logic to get the target pose from Limelight + return LimelightHelpers.getBotPose_TargetSpace(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + + public static Pose2d getBotPose2d_wpiBlue(){ + return LimelightHelpers.getBotPose2d_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker); + } + public static double getTargetRotationY(){ + if(isTargetVisible()){ + return getTargetPose()[4]; + } + else{ + return 0.00; + } + } + public static double getTargetTZ(){ + if(isTargetVisible()){ + return getTargetPose()[2]; + } + else{ + return 0.00; + } + } + @Override + public void periodic() { + SmartDashboard.putNumber("LimelightPipeIndex", LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)); + SmartDashboard.putString("LimelightPipeType", LimelightHelpers.getCurrentPipelineType(Constants.Limelight.LIMELIGHT_NAME_Intaker)); + if (isTargetVisible()){ + SmartDashboard.putString("LimelightTargetVisible", "YES"); + SmartDashboard.putNumber("LimelightTA", getTA()); + SmartDashboard.putNumber("TargetRY", getTargetRotationY()); + SmartDashboard.putNumber("TargetTX", getTX()); + SmartDashboard.putNumber("TargetTZ", getTargetTZ()); + RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); + int id = -1; + for (RawFiducial fiducial : fiducials) { + id = fiducial.id; // 标签ID + } + SmartDashboard.putNumber("LimelightTargetID", id); + + } + else { + SmartDashboard.putString("LimelightTargetVisible", "NO"); + SmartDashboard.putNumber("TargetRY", 0); + SmartDashboard.putNumber("TargetTX", 0); + SmartDashboard.putNumber("TargetTZ", 0); + SmartDashboard.putNumber("LimelightTargetID", -1); + } + + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 0fcb709..a059a36 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.InvertedValue; @@ -31,7 +32,7 @@ public class ShooterSubsystem extends SubsystemBase { private NetworkTable shooterNetworkTable; - private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0); // 初始化各个部件 @@ -94,9 +95,17 @@ public class ShooterSubsystem extends SubsystemBase { public void setConveyorSpeedByRPS(double speed) { conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度 } + public void setConveyorSpeedOpen(double speed){ + conveyorMotor.set(speed); + } //调试用 public void setBackboardSpeedByRPS(double speed){ + if (speed > 0 && speed < 10) { + speed = 10; + } else if (speed < 0 && speed > -10) { + speed = -10; + } backboardMotor.setControl(velocityRequest.withVelocity(speed)); } @@ -149,6 +158,10 @@ public class ShooterSubsystem extends SubsystemBase { public void resetbackboardencoder(){ backboardEncoder.reset(); } + public double getConveyorStatorCurrent(){ + double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble(); + return statorCurrent; + } @Override public void periodic() { @@ -160,11 +173,16 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition()); shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position); shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget()); - shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance()); + shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub()); + shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball()); + shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent()); } - public double getDistance(){ + public double getDistanceToHub(){ //获得车子距离Hub的距离 return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0); } + public double getDistanceToPassball(){ + return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0); + } }