From f68847d3e7b380068bd1a0c9d875b34b6856de6e Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Sat, 7 Mar 2026 22:24:01 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=B7=B2=E7=9F=A5=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 3 +- .../frc/robot/controls/IntakerControls.java | 10 ++-- .../frc/robot/controls/ShooterControls.java | 4 +- .../subsystems/CommandSwerveDrivetrain.java | 53 +++++++++++-------- .../robot/subsystems/IntakerSubsystem.java | 3 ++ 5 files changed, 44 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2e90fcb..9130ed8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -137,8 +137,7 @@ public final class Constants { // 单位RPS public static final double positionToleranceRotations = 0.00; //rotater容差 - public static final double IntakeGetterK = 0.3; // intake getter速度与触发器输入的比例 - public static final double Getterminspeed = 0.3; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转） + } public static class Climber { diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index e456c87..90b8738 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -77,16 +77,18 @@ public class IntakerControls { double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent(); if (controller.getLeftTriggerAxis() > 0.1) { - getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed; + getterspeed = controller.getLeftTriggerAxis() * 0.3 + 0.3; ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake); } IntakerSubsystem.setIntakerGetterSpeed(getterspeed); - + if(controller.getLeftY() < -0.5){ IntakerSubsystem.IntakerUpNonStop(); - } else if(controller.getLeftY() > 0.5){ - IntakerSubsystem.IntakerDownNonStop(); + } else if(controller.getLeftY() > 0.2){ + double rotaterspeed = controller.getLeftY() * 0.15 + 0.05; + IntakerSubsystem.setIntakerRotater(rotaterspeed); + } else { IntakerSubsystem.stopIntakerotater(); } diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index cbfc554..a6cf917 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -39,11 +39,11 @@ public class ShooterControls { case AllTelop: case PassingBall: var xP = shooterSubsystem.getDistanceToPassball(); - //xP = Math.max(1.2, Math.min(4.5, xP)); + 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; backboardPosition = xP*xP;//todo:实测传球的时候的背板位置 打表 - backboardPosition += backboardPositionOffset; + backboardPosition = -122.11889023593778*xP*xP*xP*xP*xP*xP*xP+2453.385583841308*xP*xP*xP*xP*xP*xP-20553.05413872581*xP*xP*xP*xP*xP+92963.33175750206*xP*xP*xP*xP-244905.25916147238*xP*xP*xP+375150.1363997868*xP*xP-308031.51715612336*xP+104203.52690842684; conveyorSpeed = Constants.Shooter.conveyorSpeed; if (controller.getRightTriggerAxis() < 0.1) { flywheelSpeed = 0; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index e63c6e8..9064cc7 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -353,33 +353,44 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su // 2) 旋转控制：tx 为主，ry 为辅 // tx 负责“镜头中心对准 tag” // ry 负责“机器人姿态微调” - double omegaFromTx = 0.0; - double omegaFromRy = 0.0; + // double omegaFromTx = 0.0; + // double omegaFromRy = 0.0; - if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) { - omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX; - } + // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) { + // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX; + // } - if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) { - omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY; - } + // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) { + // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY; + // } - omega = omegaFromTx + omegaFromRy; + // omega = omegaFromTx + omegaFromRy; - // 大偏角时直接给较明显的转速，先把朝向拉回来 - if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) { - omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega); - } + // // 大偏角时直接给较明显的转速，先把朝向拉回来 + // 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); - } + // // 最小输出，避免转不动 + // 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); + + Rotation2d ClimbcurrentAngle = getState().Pose.getRotation(); + double climbCurrentRad = ClimbcurrentAngle.getRadians(); + double TargetAngleforClimbing = Math.toRadians(0); + double Climberror = TargetAngleforClimbing - climbCurrentRad; + if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) { + omega = 0.0; + } else { + omega = Climberror * Constants.AutoPositioning.TurningkP; + } - // 最大输出限幅 - omega = clamp(omega, - -Constants.Limelight.AutoClimbMaxOmega, - Constants.Limelight.AutoClimbMaxOmega); // ===== 调试输出 ===== driveNetworkTable.getEntry("ClimbTX").setDouble(tx); diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index f54fdc5..ddaf147 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -101,6 +101,9 @@ public class IntakerSubsystem extends SubsystemBase{ } else { // at or above upper limit: stop stopIntakerotater(); + } } + public void setIntakerRotater(double rotaterspeed){ + intakerotater.set(rotaterspeed); } }