From 5f3452e091efc4609128f9f5c8aefe10d4f761d7 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Mon, 2 Mar 2026 14:35:18 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=8C=E6=89=8B=E6=9F=84=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 39 +++++--- .../frc/robot/controls/DriveControls.java | 65 +++---------- .../frc/robot/controls/IntakerControls.java | 97 ++++++++++--------- .../frc/robot/controls/ShooterControls.java | 73 +------------- .../robot/subsystems/ClimberSubsystem.java | 5 +- .../robot/subsystems/IntakerSubsystem.java | 3 + .../robot/subsystems/ShooterSubsystem.java | 3 + 8 files changed, 106 insertions(+), 182 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f5efb93..2a3f0fb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -52,6 +52,7 @@ public final class Constants { public static final double teleopOffsetkP = 15; public static final double autoPositioningkP = 1.0; + public static final double autoPositioningAngleError = 1.0; // 角度误差容忍度，单位为度 public static final double[] bumpY = {2.51,5.556}; public static final double[] trenchY = {0.639,7.43}; @@ -112,6 +113,8 @@ public final class Constants { // 单位RPS public static final double positionToleranceRotations = 0.00; //rotater容差 + public static final double IntakeGetterK = 1.0; // intake getter速度与触发器输入的比例 + public static final double Getterminspeed = 20; // 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 5984ec7..a653f34 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,9 +14,11 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.RobotStatus; import frc.robot.controls.DriveControls; +import frc.robot.controls.IntakerControls; import frc.robot.controls.ShooterControls; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.IntakerSubsystem; import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.tuning.ConfigTalonFXMotorTuner; @@ -32,7 +34,8 @@ public class RobotContainer { /* ====================== */ // 只使用一个 Xbox 手柄（端口 0） - private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController controllerlower = new CommandXboxController(0); + private final CommandXboxController controllerupper = new CommandXboxController(1); /* ====================== */ /* 子系统 */ @@ -42,6 +45,7 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); public final ClimberSubsystem climberSubsystem = new ClimberSubsystem(); public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + public final IntakerSubsystem IntakerSubsystem = new IntakerSubsystem(); // LED 子系统 private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72 @@ -51,9 +55,10 @@ public class RobotContainer { /* 控制封装/日志 */ /* ====================== */ - private final DriveControls driveControls = new DriveControls(drivetrain, controller,robotStatusManager ); - private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller,robotStatusManager); - private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controller); + 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 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); @@ -106,30 +111,32 @@ public class RobotContainer { // .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false))); // Back：重置场地坐标系角度 - controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); + controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); // Start：强行使用metaTag2全场定位 - controller.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2)); + controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2)); //LM 按下时使用机器人坐标系 - controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); - controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + //controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); + //controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - controller.rightStick().and(controller.leftBumper()) - .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) + controllerlower.leftTrigger() + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - controller.rightStick().and(controller.rightBumper()) - .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) + controllerlower.rightTrigger() + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); } private void configueShooter(){ shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand()); - controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1))); - controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1))); - controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100))); - controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100))); + controllerupper.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1))); + controllerupper.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1))); + controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100))); + controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100))); + controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets())); + controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand())); } /* ====================== */ /* LED 绑定 */ diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index e66a026..6141a2b 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -126,7 +126,6 @@ public class DriveControls { vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); break; - case AutoAimming: vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); @@ -161,15 +160,8 @@ public class DriveControls { errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB)); // 选误差绝对值更小的那个 double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB; - - if (Math.abs(chosenError) < Math.toRadians(2.0)) { - vomega = 0.0; - } else { - vomega = chosenError * Constants.AutoPositioning.TurningkP; - } - // 限制最大角速度 - vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega)); - + vomega = calculatevomegaFromRotationAngle(chosenError); + autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega); //vomega = calculateRotationSpeedForBump(); break; case CrossingTrench: @@ -188,14 +180,8 @@ public class DriveControls { double currentRad = currentAngle.getRadians(); double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle); double TrenchAngleerror = targetRad - currentRad; - if (Math.abs(TrenchAngleerror) < Math.toRadians(1.0)) { - vomega = 0.0; - } else { - vomega = TrenchAngleerror * Constants.AutoPositioning.TurningkP; - } - // 限幅（防止转太快） - vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega)); - + vomega = calculatevomegaFromRotationAngle(TrenchAngleerror); + autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega); //vomega = calculateRotationSpeedForTrench(); break; } @@ -238,33 +224,6 @@ public class DriveControls { double x2 = target2 - current; return Math.abs(x1) < Math.abs(x2) ? x1 : x2; } - public double calculateRotationSpeedForTrench(){ - Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 - double currentAngle = currentPose.getRotation().getDegrees(); - double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度 - double angleDifference = targetAngle - currentAngle; - // Normalize angle difference to the range [-180, 180] - if (angleDifference > 180) { - angleDifference -= 360; - } else if (angleDifference < -180) { - angleDifference += 360; - } - return calculateRotationSpeedFromRotationAngle(angleDifference); - } - public double calculateRotationSpeedForBump(){ - Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 - double currentAngle = currentPose.getRotation().getDegrees(); - double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度 - double angleDifference = targetAngle - currentAngle; - // Normalize angle difference to the range [-180, 180] - if (angleDifference > 180) { - angleDifference -= 360; - } else if (angleDifference < -180) { - angleDifference += 360; - } - return calculateRotationSpeedFromRotationAngle(angleDifference); - } - // Calculate distance and rotation (angle difference) public double[] calculateDistanceAndRotationToHub() { Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标 @@ -299,7 +258,6 @@ public class DriveControls { double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标 double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标 - // 计算目标角度（相对于场地坐标系） double deltaX = targetX - currentPose.getX(); double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2); @@ -324,7 +282,6 @@ public class DriveControls { return distanceAndRotation; } - // Calculate rotation speed from rotation angle difference public double calculateRotationSpeedFromRotationAngle(double angleDifference) { double calDifference = angleDifference; @@ -341,8 +298,18 @@ public class DriveControls { return 0; } } - - + + private double calculatevomegaFromRotationAngle(double error) { + double vomega; + if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) { + vomega = 0.0; + } else { + vomega = error * Constants.AutoPositioning.TurningkP; + } + // 限制最大角速度 + vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega)); + return vomega; + } /** * 只限制“加速”（幅值变大），不限制“减速”（幅值变小） diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index 084b021..21beeda 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -1,9 +1,13 @@ package frc.robot.controls; +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.button.CommandXboxController; import frc.robot.subsystems.IntakerSubsystem; +import frc.robot.Constants; + public class IntakerControls { private final IntakerSubsystem intakerSubsystem; @@ -16,56 +20,61 @@ public class IntakerControls { this.controller = controller; } - public void configureBindings() { + //B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转 - controller.a() - .whileTrue( - Commands.run(() -> { - // rotater下降：范围会被 subsystem 限制 - double cur = intakerSubsystem.getIntakerotaterPosition(); - intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE); + // controller.a() + // .whileTrue( + // Commands.run(() -> { + // // rotater下降：范围会被 subsystem 限制 + // double cur = intakerSubsystem.getIntakerotaterPosition(); + // intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE); - // getter 开 - intakerSubsystem.setIntakeGetterOn(true); - }, intakerSubsystem) - ) - .onFalse( - Commands.runOnce(() -> { - // 松：把当前位置作为target保持 - double cur = intakerSubsystem.getIntakerotaterPosition(); - intakerSubsystem.setIntakerRotaterPosition(cur); + // // getter 开 + // intakerSubsystem.setIntakeGetterOn(true); + // }, intakerSubsystem) + // ) + // .onFalse( + // Commands.runOnce(() -> { + // // 松：把当前位置作为target保持 + // double cur = intakerSubsystem.getIntakerotaterPosition(); + // intakerSubsystem.setIntakerRotaterPosition(cur); - // gtter继续转 - intakerSubsystem.setIntakeGetterOn(true); - }, intakerSubsystem) - ); + // // gtter继续转 + // intakerSubsystem.setIntakeGetterOn(true); + // }, intakerSubsystem) + // ); + // // === B.B：按住上升+getter停；松开后rotater保持当前位置 + // controller.b() + // .whileTrue( + // Commands.run(() -> { + // // rotater升 + // double cur = intakerSubsystem.getIntakerotaterPosition(); + // intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE); - // === B.B：按住上升+getter停；松开后rotater保持当前位置 - controller.b() - .whileTrue( - Commands.run(() -> { - // rotater升 - double cur = intakerSubsystem.getIntakerotaterPosition(); - intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE); - - // getter关 - intakerSubsystem.setIntakeGetterOn(false); - }, intakerSubsystem) - ) - .onFalse( - Commands.runOnce(() -> { - // 松：保持当前位置 - double cur = intakerSubsystem.getIntakerotaterPosition(); - intakerSubsystem.setIntakerRotaterPosition(cur); - - // getter仍然停止 - intakerSubsystem.setIntakeGetterOn(false); - }, intakerSubsystem) - ); - } + // // getter关 + // intakerSubsystem.setIntakeGetterOn(false); + // }, intakerSubsystem) + // ) + // .onFalse( + // Commands.runOnce(() -> { + // // 松：保持当前位置 + // double cur = intakerSubsystem.getIntakerotaterPosition(); + // intakerSubsystem.setIntakerRotaterPosition(cur); + // // getter仍然停止 + // intakerSubsystem.setIntakeGetterOn(false); + // }, intakerSubsystem) + // ); + public Command defaultIntakerCommand() { - return Commands.none(); + return Commands.run(() -> { + double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; + if (controller.getLeftTriggerAxis() < 0.1) { + getterspeed = 0; + } + intakerSubsystem.setIntakerGetterSpeed(getterspeed); + + }, intakerSubsystem); } } diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index 7b93dcb..dc2e387 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -110,8 +110,6 @@ public class ShooterControls { break; } - - }, shooterSubsystem); } @@ -145,73 +143,6 @@ public class ShooterControls { shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem); } - //打表参数 - // private static final double[][] FLYWHEEL_TABLE = { - // {1.5, 0.42}, - // {2.0, 0.46}, - // {2.5, 0.51}, - // {3.0, 0.57}, - // {3.5, 0.64}, - // {4.0, 0.72} - // }; - - // //线性插值函数 - // private static double lerp(double x0, double y0, double x1, double y1, double x) { - // if (Math.abs(x1 - x0) < 1e-9) { - // return y0; - // } - // double t = (x - x0) / (x1 - x0); - // return y0 + t * (y1 - y0); - // } - // private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) { - // return defaultSpeed + flywheelSpeedOffset; - // } - // private double calculateBackboardPositionOnlywithOffset(double defaultPosition) { - // return defaultPosition + backboardPositionOffset; - // } - - - // private double calculateFlywheelSpeed(double distance) { - // // 小于最小距离：用第一个点 - // if (distance <= FLYWHEEL_TABLE[0][0]) { - // return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset; - // } - // // 大于最大距离：用最后一个点 - // int last = FLYWHEEL_TABLE.length - 1; - // if (distance >= FLYWHEEL_TABLE[last][0]) { - // return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset; - // } - // // 查找区间并插值 - // for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) { - // double d0 = FLYWHEEL_TABLE[i][0]; - // double s0 = FLYWHEEL_TABLE[i][1]; - // double d1 = FLYWHEEL_TABLE[i + 1][0]; - // double s1 = FLYWHEEL_TABLE[i + 1][1]; - // if (distance >= d0 && distance <= d1) { - // return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset; - // } - // } - // return 0; - // } - - // private double calculateBackboardPosition(double distance) { - // double BackboardPosition; - // // 距离阈值（单位自己按distance进行修改） - // final double DISTANCE_NEAR = 2.0; // 近距离阈值 - // final double DISTANCE_MID = 4.0; // 中距离阈值 - // if (distance < DISTANCE_NEAR) { - // BackboardPosition = Constants.ShooterCalculation.positionNear; - // } - // else if (distance < DISTANCE_MID) { - // BackboardPosition = Constants.ShooterCalculation.positionMid; - // } - // else { - // BackboardPosition = Constants.ShooterCalculation.postionFar; - // } - // BackboardPosition += backboardPositionOffset; - // return BackboardPosition; - // } - public void adjustFlywheelSpeedOffset(double offset) { flywheelSpeedOffset += offset; } @@ -219,4 +150,8 @@ public class ShooterControls { public void adjustBackboardRateOffset(double offset) { backboardPositionOffset += offset; } + public void resetOffsets(){ + flywheelSpeedOffset = 0.0; + backboardPositionOffset = 0.0; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 051a3b1..76e2531 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -46,12 +46,10 @@ public class ClimberSubsystem extends SubsystemBase { holdPositionTicks = getPositionTicks(); } public void initialize() { - climberMotor.set(-0.07); - } + public void zeroEncoder(){ - climberMotor.setPosition(0); stopMotor(); } @@ -75,7 +73,6 @@ public class ClimberSubsystem extends SubsystemBase { } public void climbDownNonStop(){ - deltaPositionTicks = Constants.Climber.deltaDownTicks; climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks-deltaPositionTicks)); } diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index dcaa232..92632a3 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -64,4 +64,7 @@ public class IntakerSubsystem extends SubsystemBase{ intakegetter.setControl(velocityRequest.withVelocity(0)); } } + public void setIntakerGetterSpeed(double speed) { + intakegetter.setControl(velocityRequest.withVelocity(speed)); + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 99eb08a..0fcb709 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -146,6 +146,9 @@ public class ShooterSubsystem extends SubsystemBase { public void conveyorRun(){ setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed); } + public void resetbackboardencoder(){ + backboardEncoder.reset(); + } @Override public void periodic() {