From aa323814de45da95b18e8d677e37600d528ef9f1 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Tue, 3 Mar 2026 21:12:34 +0800 Subject: [PATCH 01/16] =?UTF-8?q?=E5=BC=80=E7=8E=AFintaker=20=E7=BA=A2?= =?UTF-8?q?=E8=93=9DDrivercontrol=EF=BC=88=E5=BE=85=E4=BC=A0=E8=93=9D?= =?UTF-8?q?=E6=96=B9=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 9 +++++--- .../frc/robot/controls/DriveControls.java | 7 +++--- .../frc/robot/controls/IntakerControls.java | 4 ++-- .../robot/subsystems/IntakerSubsystem.java | 23 +++---------------- 4 files changed, 15 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c906349..0da3842 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -88,9 +88,12 @@ public final class Constants { 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 BlueHubPositionX = 0; + public static final double BlueHubPositionY = 0; public static final double PassingBallPosY1 = 2.51; public static final double PassingBallPosY2 = 5.556; + public static final double RedPassingBallPosX = 0; + public static final double BluePassingBallPosX = 0; } @@ -111,12 +114,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/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index cf3d087..1b8779f 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; public class DriveControls { + public boolean isRedAlliance; private final CommandSwerveDrivetrain drivetrain; @@ -234,8 +235,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(); @@ -262,7 +263,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 坐标 diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index b93de5b..e63b99d 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -76,9 +76,9 @@ public class IntakerControls { IntakerSubsystem.setIntakerGetterSpeed(getterspeed); if(controller.getLeftY() < -0.5){ - IntakerSubsystem.IntakerDownNonStop(); - } else if(controller.getLeftY() > 0.5){ IntakerSubsystem.IntakerUpNonStop(); + } else if(controller.getLeftY() > 0.5){ + IntakerSubsystem.IntakerDownNonStop(); } else { IntakerSubsystem.stopIntakerotater(); } diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index 603947b..9fd211f 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.1);} - - // if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) { - // intakerotater.setControl(velocityRequest.withVelocity(-Constants.Intaker.intakeRotaterVelocity)); - // } else { - // stopIntakerotater(); - // } + intakerotater.set(0.2); } } From 042d483b16cd75b3c3969ee1353cd085f9909f1a Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Thu, 5 Mar 2026 12:18:12 +0800 Subject: [PATCH 02/16] =?UTF-8?q?=E7=94=B5=E5=8E=8B=E9=97=AD=E7=8E=AF?= =?UTF-8?q?=E7=9A=84shooter=20=E5=8F=8C=E6=93=8D=E5=9D=87=E5=8F=AF?= =?UTF-8?q?=E8=A7=A6=E5=8F=91autoaim=20intaker=E5=B7=B2=E9=AA=8C=E8=AF=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 14 ++++++++++---- .../java/frc/robot/controls/ClimberControls.java | 4 ++-- .../java/frc/robot/controls/IntakerControls.java | 8 +++++++- .../java/frc/robot/controls/ShooterControls.java | 6 ++---- .../frc/robot/subsystems/ShooterSubsystem.java | 3 ++- 6 files changed, 26 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0da3842..4989896 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,14 +15,14 @@ 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) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 510bfba..5d1821c 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,14 @@ 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 + Trigger autoAim = controllerupper.rightBumper().or(controllerlower.leftStick()); + autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); + autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); controllerlower.leftTrigger() .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) diff --git a/src/main/java/frc/robot/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java index 543aff9..6ee58ab 100644 --- a/src/main/java/frc/robot/controls/ClimberControls.java +++ b/src/main/java/frc/robot/controls/ClimberControls.java @@ -24,9 +24,9 @@ public class ClimberControls { return Commands.run(()->{ //climber.climbUp(); if(controller.getRightY() < -0.5){ - climber.climbDownNonStop(); - } else if(controller.getRightY() > 0.5){ climber.climbUpNonStop(); + } else if(controller.getRightY() > 0.5){ + climber.climbDownNonStop(); } else if (controller.y().getAsBoolean()) { climber.climbUp(); } else if (controller.x().getAsBoolean()) { diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index e63b99d..75d1b73 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; } @@ -70,10 +73,13 @@ public class IntakerControls { public Command defaultIntakerCommand() { return Commands.run(() -> { double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; + double conveyoyspeedforintake = 10; if (controller.getLeftTriggerAxis() < 0.1) { getterspeed = 0; + conveyoyspeedforintake = 0; } IntakerSubsystem.setIntakerGetterSpeed(getterspeed); + ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake); if(controller.getLeftY() < -0.5){ IntakerSubsystem.IntakerUpNonStop(); diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index 4d2ae42..c74d9d8 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; @@ -78,12 +79,9 @@ public class ShooterControls { flywheelSpeed = 0; conveyorSpeed = 0; } - if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() Date: Thu, 5 Mar 2026 12:39:53 +0800 Subject: [PATCH 03/16] =?UTF-8?q?=E4=BC=A0=E5=85=A5=E4=BA=86=E6=89=80?= =?UTF-8?q?=E6=9C=89=E5=9C=BA=E5=9C=B0=E5=9D=90=E6=A0=87=E5=8F=82=E6=95=B0?= =?UTF-8?q?=20=E4=B8=8A=E5=B1=82=E6=93=8D=E4=BD=9C=E6=89=8B=E5=8F=AF?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E5=BA=95=E7=9B=98=E7=9E=84=E5=87=86=20?= =?UTF-8?q?=E4=BC=A0=E7=90=83=E5=BE=85=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 12 ++++++------ src/main/java/frc/robot/RobotContainer.java | 9 +++++++-- .../java/frc/robot/controls/ShooterControls.java | 3 --- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4989896..abab723 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -88,12 +88,12 @@ public final class Constants { public class Field { public static final double RedHubPositionX = 11.914; public static final double RedHubPositionY = 4.034; - public static final double BlueHubPositionX = 0; - public static final double BlueHubPositionY = 0; - public static final double PassingBallPosY1 = 2.51; - public static final double PassingBallPosY2 = 5.556; - public static final double RedPassingBallPosX = 0; - public static final double BluePassingBallPosX = 0; + 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; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d1821c..1b6c243 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,11 +123,16 @@ public class RobotContainer { // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - //双操均可触发autoaimming - Trigger autoAim = controllerupper.rightBumper().or(controllerlower.leftStick()); + //双操均可触发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)) .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index c74d9d8..50fef46 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -52,9 +52,6 @@ public class ShooterControls { if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() Date: Thu, 5 Mar 2026 15:41:05 +0800 Subject: [PATCH 04/16] =?UTF-8?q?=E7=BB=86=E5=8C=96=E4=BC=A0=E7=90=83=20?= =?UTF-8?q?=E5=BE=85=E6=89=93=E8=A1=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 5 ----- src/main/java/frc/robot/controls/DriveControls.java | 4 +++- src/main/java/frc/robot/controls/ShooterControls.java | 4 ++-- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 7 +++++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index abab723..8793572 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,11 +39,6 @@ public final class Constants { 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; diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 1b8779f..b273d0a 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -124,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; diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index 50fef46..48bcfc3 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -38,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; @@ -65,7 +65,7 @@ public class ShooterControls { } break; case AutoAimming: - var x = shooterSubsystem.getDistance(); + var x = shooterSubsystem.getDistanceToHub(); 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; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 47f38b0..2bf1cf0 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -161,11 +161,14 @@ 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()); } - 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); + } } From ebcf6ff3c3ad92e87f85d71a668f8ccd6a2b54c3 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Thu, 5 Mar 2026 16:48:24 +0800 Subject: [PATCH 05/16] for limelight --- src/main/java/frc/robot/Constants.java | 3 + .../subsystems/CommandSwerveDrivetrain.java | 75 ++++++++++- .../robot/subsystems/LimelightSupplier.java | 117 ++++++++++++++++++ 3 files changed, 193 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/LimelightSupplier.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8793572..a05d6dc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,6 +78,9 @@ 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 Pipeline = 1; + public static final double TXDeadHand = 0.5; + public static final double TZDeadHand = 0.5; } public class Field { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 46987f0..ffda559 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -127,12 +128,82 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su setControl( new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled) - ); + ); } + // public String driveToAprilTagOfClimber(double speed) { + // // 记录当前方向，作为目标方向，避免在驱动过程中偏航 + // Rotation2d TargetHeading=getPose().getRotation(); + // // 设置 Limelight pipeline（25赛季只有一条，直接设置） + // LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker,Constants.Limelight.Pipeline); + // // 如果 Limelight 没有检测到目标，直接返回 + // if (!LimelightSupplier.isTargetVisible()) { + // return "No Target"; + // } + // // 获取检测到的 AprilTag ID + // int trueId = LimelightSupplier.getAprilTagID(); + + // // 只有特定的 Tag ID 被认为是目标 + // int[] validTagIDs = {16,32}; + // boolean isValidTag = false; + // for (int id : validTagIDs) { + // if (id == trueId) { + // isValidTag = true; + // break; + // } + // } + // if (!isValidTag) { + // return "No Target"; + // } + + // // 获取 Limelight 的偏移数据 + // double tx = LimelightSupplier.getTX(); // 水平偏移角度 + // double targetTz = -LimelightSupplier.getTargetTZ(); // 目标与底盘前方的距离，取负方便坐标系 + // double ry = LimelightSupplier.getTargetRotationY(); // 目标旋转角 + + // // 根据速度参数计算实际驱动速度 + // double multiply = Constants.Swerve.maxSpeed * speed; + + // // 水平偏移死区处理 + // if (Math.abs(tx) < Constants.Limelight.TXDeadHand) { + // tx = 0; + // } else { + // tx *= multiply; + // } + + // // 前进距离死区处理 + // if (Math.abs(targetTz) < Constants.Limelight.TZDeadHand) { + // targetTz = 0; + // } else { + // targetTz *= multiply; + // targetTz *= 5; // 放大系数，增加前进响应 + // } + + // // 旋转角度死区处理 + // if (Math.abs(ry) < Constants.Swerve.reefCorrection.roatationYDeadHand) { + // ry = 0; + // } else { + // ry /= 10; // 缩小旋转响应 + // ry = Math.min(ry, Constants.Swerve.maxAngularVelocity); + // ry = Math.max(ry, -Constants.Swerve.maxAngularVelocity); + // } + + // // 输出到 SmartDashboard 方便调试 + // SmartDashboard.putNumber("driveTx_r", tx); + // SmartDashboard.putNumber("driveTz_r", targetTz); + // SmartDashboard.putNumber("driveRy_r", ry); + + // // 调用底盘驱动方法 + // drive( + // new Translation2d(tx, targetTz), // x = 水平, y = 前进 + // ry, // 旋转速度 + // false, // 是否相对场地驱动 + // false // 是否速度保持为绝对速度 + // ); + // return "Driving to AprilTag"; + // } - private static final double kMaxVisionJumpMeters = 1.0; private static final double kMaxOmegaDegPerSec = 360.0; 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..b2ed76e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightSupplier.java @@ -0,0 +1,117 @@ +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(1); + } + + 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("LimelightTX", getTX()); + // SmartDashboard.putNumber("LimelightTY", getTY()); + SmartDashboard.putNumber("LimelightTA", getTA()); + // double[] targetPose = getTargetPose(); + // String s = ""; + // for(double x : targetPose){ + // s += Double.toString(x); + // s += " "; + // } + // SmartDashboard.putString("LimeLightTP", s); + 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); + } + + } +} From b236f43f4d9051497d6f8953040feca3975b9efe Mon Sep 17 00:00:00 2001 From: Alyssum-chenyiyi <13162501939@163.com> Date: Thu, 5 Mar 2026 18:57:57 +0800 Subject: [PATCH 06/16] climb auto --- src/main/java/frc/robot/Constants.java | 3 + .../subsystems/CommandSwerveDrivetrain.java | 70 ++++++++++++ .../robot/subsystems/LimelightSupplier.java | 108 ++++++++++++++++++ 3 files changed, 181 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/LimelightSupplier.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8793572..1b6c31d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,6 +78,9 @@ 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 double AutoClimbAlignSpeed = 0.5;//等查资料 + public static final double AutoClimbAlignTolerance = 0.1;//？ + public static final double AutoClimbmaxAngularVelocity = 0.5;//等查资料 } public class Field { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 46987f0..c7f845e 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -130,6 +131,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su ); } + + @@ -276,6 +279,73 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return true; } + + public String driveToAprilTag(){ + //Rotation2d targetHeading = getPose().getRotation(); + if(!LimelightSupplier.isTargetVisible() ){ + return "No Target"; + } + boolean ifRightTag = false; + int trueId = LimelightSupplier.getAprilTagID(); + for(int id : new int[]{16,32}){ + if(id == trueId){ + ifRightTag = true; + break; + } + } + if(!ifRightTag){ + return "No Target"; + } + double tx = LimelightSupplier.getTX() ; + double targetTz = -LimelightSupplier.getTargetTZ(); + double ry = LimelightSupplier.getTargetRotationY(); + + double multiply = Constants.Limelight.AutoClimbAlignSpeed; + if(Math.abs(tx) Date: Thu, 5 Mar 2026 19:21:24 +0800 Subject: [PATCH 07/16] climber --- src/main/java/frc/robot/RobotContainer.java | 6 ++++++ src/main/java/frc/robot/controls/DriveControls.java | 1 + 2 files changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b6c243..ad56eb3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,6 +140,12 @@ 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/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index b273d0a..7958cbd 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -145,6 +145,7 @@ public class DriveControls { autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); break; case Climbing: + drivetrain.driveToAprilTag(); break; case CrossingBump: double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]); From f7eb1d7d26afe67e5b8014dfd7111463b0480528 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Thu, 5 Mar 2026 19:29:06 +0800 Subject: [PATCH 08/16] =?UTF-8?q?=E5=BA=95=E7=9B=98=E4=BF=AE=E6=AD=A3ing?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 6 +- .../subsystems/CommandSwerveDrivetrain.java | 75 +------------------ .../robot/subsystems/ShooterSubsystem.java | 5 ++ 3 files changed, 9 insertions(+), 77 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a05d6dc..06a788c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,15 +26,15 @@ public final class Constants { 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; // 背板下限角度（根据实际需求设置） diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index ffda559..3189e76 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -130,80 +130,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled) ); } - - // public String driveToAprilTagOfClimber(double speed) { - // // 记录当前方向，作为目标方向，避免在驱动过程中偏航 - // Rotation2d TargetHeading=getPose().getRotation(); - // // 设置 Limelight pipeline（25赛季只有一条，直接设置） - // LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker,Constants.Limelight.Pipeline); - - // // 如果 Limelight 没有检测到目标，直接返回 - // if (!LimelightSupplier.isTargetVisible()) { - // return "No Target"; - // } - // // 获取检测到的 AprilTag ID - // int trueId = LimelightSupplier.getAprilTagID(); - - // // 只有特定的 Tag ID 被认为是目标 - // int[] validTagIDs = {16,32}; - // boolean isValidTag = false; - // for (int id : validTagIDs) { - // if (id == trueId) { - // isValidTag = true; - // break; - // } - // } - // if (!isValidTag) { - // return "No Target"; - // } - - // // 获取 Limelight 的偏移数据 - // double tx = LimelightSupplier.getTX(); // 水平偏移角度 - // double targetTz = -LimelightSupplier.getTargetTZ(); // 目标与底盘前方的距离，取负方便坐标系 - // double ry = LimelightSupplier.getTargetRotationY(); // 目标旋转角 - - // // 根据速度参数计算实际驱动速度 - // double multiply = Constants.Swerve.maxSpeed * speed; - - // // 水平偏移死区处理 - // if (Math.abs(tx) < Constants.Limelight.TXDeadHand) { - // tx = 0; - // } else { - // tx *= multiply; - // } - - // // 前进距离死区处理 - // if (Math.abs(targetTz) < Constants.Limelight.TZDeadHand) { - // targetTz = 0; - // } else { - // targetTz *= multiply; - // targetTz *= 5; // 放大系数，增加前进响应 - // } - - // // 旋转角度死区处理 - // if (Math.abs(ry) < Constants.Swerve.reefCorrection.roatationYDeadHand) { - // ry = 0; - // } else { - // ry /= 10; // 缩小旋转响应 - // ry = Math.min(ry, Constants.Swerve.maxAngularVelocity); - // ry = Math.max(ry, -Constants.Swerve.maxAngularVelocity); - // } - - // // 输出到 SmartDashboard 方便调试 - // SmartDashboard.putNumber("driveTx_r", tx); - // SmartDashboard.putNumber("driveTz_r", targetTz); - // SmartDashboard.putNumber("driveRy_r", ry); - - // // 调用底盘驱动方法 - // drive( - // new Translation2d(tx, targetTz), // x = 水平, y = 前进 - // ry, // 旋转速度 - // false, // 是否相对场地驱动 - // false // 是否速度保持为绝对速度 - // ); - // return "Driving to AprilTag"; - // } - + private static final double kMaxVisionJumpMeters = 1.0; private static final double kMaxOmegaDegPerSec = 360.0; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2bf1cf0..68e49a9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -98,6 +98,11 @@ public class ShooterSubsystem extends SubsystemBase { //调试用 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)); } From 0da9ee5f36c8ca510e6ad4ffb907d1bc5c1a3e72 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Thu, 5 Mar 2026 19:34:24 +0800 Subject: [PATCH 09/16] climber --- src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 4 ++ .../frc/robot/controls/DriveControls.java | 1 + .../subsystems/CommandSwerveDrivetrain.java | 68 ++++++++++++++++++- 4 files changed, 75 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 06a788c..dcafbe3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -81,6 +81,9 @@ public final class Constants { public static final int Pipeline = 1; public static final double TXDeadHand = 0.5; public static final double TZDeadHand = 0.5; + public static final double AutoClimbAlignSpeed = 0.1; + public static final double AutoClimbAlignTolerance = 0.1; + public static final double AutoClimbmaxAngularVelocity = 0.1; } public class Field { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b6c243..c0a8a3d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,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/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index b273d0a..7958cbd 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -145,6 +145,7 @@ public class DriveControls { autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); break; case Climbing: + drivetrain.driveToAprilTag(); break; case CrossingBump: double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 3189e76..0813b5d 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -130,7 +130,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled) ); } - + private static final double kMaxVisionJumpMeters = 1.0; private static final double kMaxOmegaDegPerSec = 360.0; @@ -274,6 +274,72 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return true; } + public String driveToAprilTag(){ + //Rotation2d targetHeading = getPose().getRotation(); + if(!LimelightSupplier.isTargetVisible() ){ + return "No Target"; + } + boolean ifRightTag = false; + int trueId = LimelightSupplier.getAprilTagID(); + for(int id : new int[]{16,32}){ + if(id == trueId){ + ifRightTag = true; + break; + } + } + if(!ifRightTag){ + return "No Target"; + } + double tx = LimelightSupplier.getTX() ; + double targetTz = -LimelightSupplier.getTargetTZ(); + double ry = LimelightSupplier.getTargetRotationY(); + + double multiply = Constants.Limelight.AutoClimbAlignSpeed; + if(Math.abs(tx) Date: Thu, 5 Mar 2026 20:28:43 +0800 Subject: [PATCH 10/16] =?UTF-8?q?conveyor=E6=9C=89bug=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/controls/ShooterControls.java | 4 ++-- .../java/frc/robot/subsystems/CommandSwerveDrivetrain.java | 2 ++ src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++++ 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dcafbe3..f4897e2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,7 +15,7 @@ public final class Constants { CrossingTrench } public class Shooter { - public static final double conveyorSpeed = 25; + public static final double conveyorSpeed = 50; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) .withKV(0.099).withKA(0.0).withKS(0.0); diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index 48bcfc3..bb2bd21 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -76,14 +76,14 @@ public class ShooterControls { flywheelSpeed = 0; conveyorSpeed = 0; } - if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() Date: Thu, 5 Mar 2026 20:44:11 +0800 Subject: [PATCH 11/16] 1 --- src/main/java/frc/robot/Constants.java | 9 --------- src/main/java/frc/robot/RobotContainer.java | 5 ----- .../frc/robot/controls/ShooterControls.java | 6 +++--- .../subsystems/CommandSwerveDrivetrain.java | 18 ------------------ .../robot/subsystems/LimelightSupplier.java | 13 ------------- 5 files changed, 3 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 734ecea..6766a9b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,18 +78,9 @@ 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; -<<<<<<< HEAD - public static final int Pipeline = 1; - public static final double TXDeadHand = 0.5; - public static final double TZDeadHand = 0.5; - public static final double AutoClimbAlignSpeed = 0.1; - public static final double AutoClimbAlignTolerance = 0.1; - public static final double AutoClimbmaxAngularVelocity = 0.1; -======= public static final double AutoClimbAlignSpeed = 0.5;//等查资料 public static final double AutoClimbAlignTolerance = 0.1;//？ public static final double AutoClimbmaxAngularVelocity = 0.5;//等查资料 ->>>>>>> a8acbce9dc302adc52e28e6f8c205677f50ea20e } public class Field { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7a76b0..c0a8a3d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -144,11 +144,6 @@ public class RobotContainer { controllerlower.a() .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing)) .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); -<<<<<<< HEAD -======= - - ->>>>>>> a8acbce9dc302adc52e28e6f8c205677f50ea20e } private void configueShooter(){ diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index bb2bd21..157d292 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -76,9 +76,9 @@ public class ShooterControls { flywheelSpeed = 0; conveyorSpeed = 0; } - if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()>>>>>> a8acbce9dc302adc52e28e6f8c205677f50ea20e private static final double kMaxVisionJumpMeters = 1.0; private static final double kMaxOmegaDegPerSec = 360.0; @@ -282,10 +274,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return true; } -<<<<<<< HEAD -======= - ->>>>>>> a8acbce9dc302adc52e28e6f8c205677f50ea20e public String driveToAprilTag(){ //Rotation2d targetHeading = getPose().getRotation(); if(!LimelightSupplier.isTargetVisible() ){ @@ -296,18 +284,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su for(int id : new int[]{16,32}){ if(id == trueId){ ifRightTag = true; -<<<<<<< HEAD System.out.println("Target get."); -======= ->>>>>>> a8acbce9dc302adc52e28e6f8c205677f50ea20e break; } } if(!ifRightTag){ -<<<<<<< HEAD System.out.println("No Target."); -======= ->>>>>>> a8acbce9dc302adc52e28e6f8c205677f50ea20e return "No Target"; } double tx = LimelightSupplier.getTX() ; diff --git a/src/main/java/frc/robot/subsystems/LimelightSupplier.java b/src/main/java/frc/robot/subsystems/LimelightSupplier.java index 9c21a34..5d4ec72 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSupplier.java +++ b/src/main/java/frc/robot/subsystems/LimelightSupplier.java @@ -84,20 +84,7 @@ public class LimelightSupplier extends SubsystemBase { SmartDashboard.putString("LimelightPipeType", LimelightHelpers.getCurrentPipelineType(Constants.Limelight.LIMELIGHT_NAME_Intaker)); if (isTargetVisible()){ SmartDashboard.putString("LimelightTargetVisible", "YES"); -<<<<<<< HEAD - // SmartDashboard.putNumber("LimelightTX", getTX()); - // SmartDashboard.putNumber("LimelightTY", getTY()); SmartDashboard.putNumber("LimelightTA", getTA()); - // double[] targetPose = getTargetPose(); - // String s = ""; - // for(double x : targetPose){ - // s += Double.toString(x); - // s += " "; - // } - // SmartDashboard.putString("LimeLightTP", s); -======= - SmartDashboard.putNumber("LimelightTA", getTA()); ->>>>>>> a8acbce9dc302adc52e28e6f8c205677f50ea20e SmartDashboard.putNumber("TargetRY", getTargetRotationY()); SmartDashboard.putNumber("TargetTX", getTX()); SmartDashboard.putNumber("TargetTZ", getTargetTZ()); From ce8b17e37712df73428d546b86b9cb5536316c56 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Fri, 6 Mar 2026 14:32:16 +0800 Subject: [PATCH 12/16] =?UTF-8?q?conveyor=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/controls/IntakerControls.java | 11 +++++------ src/main/java/frc/robot/controls/ShooterControls.java | 8 ++++---- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6766a9b..519e5a6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,7 +15,7 @@ public final class Constants { CrossingTrench } public class Shooter { - public static final double conveyorSpeed = 50; + public static final double conveyorSpeed =25; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) .withKV(0.099).withKA(0.0).withKS(0.0); diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index 75d1b73..b4cc3cb 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -72,14 +72,13 @@ public class IntakerControls { public Command defaultIntakerCommand() { return Commands.run(() -> { - double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; - double conveyoyspeedforintake = 10; - if (controller.getLeftTriggerAxis() < 0.1) { - getterspeed = 0; - conveyoyspeedforintake = 0; + double getterspeed = 0; + double conveyoyspeedforintake = 15; + if (controller.getLeftTriggerAxis() > 0.1) { + //getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; + ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake); } IntakerSubsystem.setIntakerGetterSpeed(getterspeed); - ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake); if(controller.getLeftY() < -0.5){ IntakerSubsystem.IntakerUpNonStop(); diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index 157d292..cbfc554 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -49,7 +49,7 @@ public class ShooterControls { flywheelSpeed = 0; conveyorSpeed = 0; } - if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() Date: Fri, 6 Mar 2026 15:03:26 +0800 Subject: [PATCH 13/16] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=B7=B2=E7=9F=A5?= =?UTF-8?q?=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/controls/IntakerControls.java | 4 +++- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 5 +++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index b4cc3cb..2cff233 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -74,10 +74,12 @@ public class IntakerControls { return Commands.run(() -> { double getterspeed = 0; double conveyoyspeedforintake = 15; + double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent(); if (controller.getLeftTriggerAxis() > 0.1) { - //getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; + 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/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 64235f7..a059a36 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -158,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() { @@ -171,6 +175,7 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget()); shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub()); shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball()); + shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent()); } public double getDistanceToHub(){ From f8fc03795920bb837bcbc022c0acf77bdf47f759 Mon Sep 17 00:00:00 2001 From: Alyssum-chenyiyi <13162501939@163.com> Date: Fri, 6 Mar 2026 17:27:42 +0800 Subject: [PATCH 14/16] climber limelight --- src/main/java/frc/robot/Constants.java | 2 ++ .../java/frc/robot/controls/Autocommand.java | 20 +++++++++++++++ .../frc/robot/controls/ClimberControls.java | 8 ++++++ .../frc/robot/controls/DriveControls.java | 16 ++++++++++-- .../subsystems/CommandSwerveDrivetrain.java | 25 +++++++++++++------ 5 files changed, 61 insertions(+), 10 deletions(-) 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 7958cbd..171b45b 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]); @@ -394,6 +395,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 9979ef6..a431bc9 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; @@ -320,13 +328,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su ry = Math.min(ry, Constants.Limelight.AutoClimbmaxAngularVelocity); ry = Math.max(ry, -Constants.Limelight.AutoClimbmaxAngularVelocity); } - - - positioningNetworkTable.getEntry("interestPointsClimb tx").setDouble(tx); - positioningNetworkTable.getEntry("interestPointsClimb targetTz").setDouble(targetTz); - positioningNetworkTable.getEntry("interestPointsClimb ry").setDouble(ry); - - + 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 不变 @@ -342,6 +344,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return "Driving to AprilTag"; } + public boolean isAlignedToAprilTag(){ + return isAlignedToAprilTag; + } @Override public void periodic() { @@ -353,7 +358,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, From 83c06bd0e69134d7dff654a0102bcae0d0c921f7 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Fri, 6 Mar 2026 17:29:43 +0800 Subject: [PATCH 15/16] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=BA=95=E7=9B=98?= =?UTF-8?q?=E8=87=AA=E5=8A=A8=E7=9E=84=E5=87=86trench=E5=92=8Cbmp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/controls/DriveControls.java | 5 ++++- src/main/java/frc/robot/controls/IntakerControls.java | 1 + .../frc/robot/subsystems/CommandSwerveDrivetrain.java | 9 ++++----- src/main/java/frc/robot/subsystems/IntakerSubsystem.java | 2 +- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 7958cbd..0bbb085 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -157,6 +157,7 @@ public class DriveControls { Math.max(Math.abs(raw), 0.4), raw ); + vy = isRedAlliance ? -vy : vy; } autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump); @@ -186,6 +187,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(); @@ -209,7 +211,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) diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index 2cff233..e456c87 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -75,6 +75,7 @@ public class IntakerControls { 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); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 9979ef6..b839316 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -274,6 +274,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return true; } + public void setLimelightIntakerPipeline(int pipeline){ + LimelightSupplier.setPipeline(pipeline); + } + public String driveToAprilTag(){ //Rotation2d targetHeading = getPose().getRotation(); if(!LimelightSupplier.isTargetVisible() ){ @@ -322,11 +326,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } - positioningNetworkTable.getEntry("interestPointsClimb tx").setDouble(tx); - positioningNetworkTable.getEntry("interestPointsClimb targetTz").setDouble(targetTz); - positioningNetworkTable.getEntry("interestPointsClimb ry").setDouble(ry); - - ChassisSpeeds scaled = new ChassisSpeeds( tx, // X 不变 targetTz, // Y 不变 diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index 9fd211f..61f4299 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -86,6 +86,6 @@ public class IntakerSubsystem extends SubsystemBase{ intakerotater.set(-0.05); } public void IntakerUpNonStop(){ - intakerotater.set(0.2); + intakerotater.set(0.1); } } From 6fe9613b67fc4e656c1b090c8201518da9855243 Mon Sep 17 00:00:00 2001 From: Alyssum-chenyiyi <13162501939@163.com> Date: Fri, 6 Mar 2026 19:40:10 +0800 Subject: [PATCH 16/16] climber limelight 0306 --- src/main/java/frc/robot/Constants.java | 25 ++- .../java/frc/robot/controls/Autocommand.java | 2 +- .../frc/robot/controls/DriveControls.java | 2 + .../subsystems/CommandSwerveDrivetrain.java | 177 +++++++++++------- .../robot/subsystems/LimelightSupplier.java | 2 +- 5 files changed, 140 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b12add4..2e90fcb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -80,9 +80,28 @@ public final class Constants { 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;//等查资料 + + // 误差死区 / 对齐判定 + 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 { diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index 22ad966..a1d9342 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -76,7 +76,7 @@ import frc.robot.Constants.RobotStatus; ).until(() -> drivetrain.isAlignedToAprilTag()) ); } - + } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 171b45b..a53fc9b 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -135,10 +135,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; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index a431bc9..e63c6e8 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Constants.DriveMode; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.subsystems.LimelightHelpers.RawFiducial; import frc.robot.Constants; import edu.wpi.first.networktables.*; @@ -269,81 +270,131 @@ 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; } - public String driveToAprilTag(){ - //Rotation2d targetHeading = getPose().getRotation(); - if(!LimelightSupplier.isTargetVisible() ){ - return "No Target"; - } - boolean ifRightTag = false; - int trueId = LimelightSupplier.getAprilTagID(); - for(int id : new int[]{16,32}){ - if(id == trueId){ - ifRightTag = true; - System.out.println("Target get."); - break; - } - } - if(!ifRightTag){ - System.out.println("No Target."); - return "No Target"; - } - double tx = LimelightSupplier.getTX() ; - double targetTz = -LimelightSupplier.getTargetTZ(); - double ry = LimelightSupplier.getTargetRotationY(); - - double multiply = Constants.Limelight.AutoClimbAlignSpeed; - if(Math.abs(tx) 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; } diff --git a/src/main/java/frc/robot/subsystems/LimelightSupplier.java b/src/main/java/frc/robot/subsystems/LimelightSupplier.java index 5d4ec72..7836b78 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSupplier.java +++ b/src/main/java/frc/robot/subsystems/LimelightSupplier.java @@ -17,7 +17,7 @@ import frc.robot.subsystems.LimelightHelpers.RawFiducial; public class LimelightSupplier extends SubsystemBase { public LimelightSupplier() { - setPipeline(1); + setPipeline(8); } public static double getTX() {