diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9130ed8..795dfda 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,5 +1,7 @@ package frc.robot; +import javax.net.ssl.TrustManager; + import com.ctre.phoenix6.configs.Slot0Configs; import edu.wpi.first.math.geometry.Pose2d; @@ -77,7 +79,7 @@ public final class Constants { public class Limelight { 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 Boolean UsingMetaTag2 = true; // 是否使用 MetaTag2(如果你们的 pipeline 配置了这个功能的话) public static final int climbPipelineIndex = 8; public static final int locatePipelineIndex = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c0a8a3d..3ad44cc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -117,19 +117,19 @@ public class RobotContainer { controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); // Start:强行使用metaTag2全场定位 - controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2)); + controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2)); //LM 按下时autoaimming,松开时alltelop // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); //双操均可触发autoaimming模式,松开时alltelop - Trigger autoAim = controllerupper.rightBumper().or(controllerlower.rightStick()); + 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()); + Trigger autoAim2 = controllerupper.leftBumper();//.or(controllerlower.leftStick()); autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall)); autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index a1d9342..769702e 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -1,5 +1,7 @@ package frc.robot.controls; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -19,7 +21,23 @@ import frc.robot.Constants; import frc.robot.Constants.RobotStatus; public class Autocommand extends SequentialCommandGroup{ - + + public static void preNameCommands( + ClimberSubsystem climber, + IntakerSubsystem intaker, + CommandSwerveDrivetrain drivetrain, + ShooterSubsystem shooter + ) { + NamedCommands.registerCommand("climb_up", climb_up(climber)); + NamedCommands.registerCommand("climb_down", climb_down(climber)); + NamedCommands.registerCommand("start_intake", start_intake(intaker)); + NamedCommands.registerCommand("stop_intake", stop_intake(intaker)); + NamedCommands.registerCommand("intaker_down", intaker_down(intaker)); + NamedCommands.registerCommand("intaker_up", intaker_up(intaker)); + NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain)); + NamedCommands.registerCommand("shoot", shoot(shooter)); + } + public static Command climb_up(ClimberSubsystem climber) { return new RunCommand(() -> { climber.climbUp(); // 控制爬升器上升 @@ -73,12 +91,23 @@ import frc.robot.Constants.RobotStatus; LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); }, drivetrain - ).until(() -> drivetrain.isAlignedToAprilTag()) - ); - } + ).until(() -> drivetrain.isAlignedToAprilTag())); + } + + public static Command shoot(ShooterSubsystem shooter) { + return new InstantCommand(() -> { + shooter.setFlywheelSpeedByRPS(60); + shooter.setBackboardPosition(200); // 设置自动射击转速 + }, shooter).withTimeout(1.0).andThen(new InstantCommand(() -> { + shooter.setConveyorSpeedByRPS(25); + }, shooter).withTimeout(5.0).andThen(new InstantCommand(() -> { + shooter.setFlywheelSpeedByRPS(0); // 停止射击 + shooter.setBackboardPosition(0); // 恢复背板位置 + shooter.setConveyorSpeedByRPS(0); // 停止输送 + }, shooter))); - + } } diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index 90b8738..687eff4 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -85,9 +85,10 @@ public class IntakerControls { if(controller.getLeftY() < -0.5){ IntakerSubsystem.IntakerUpNonStop(); - } else if(controller.getLeftY() > 0.2){ - double rotaterspeed = controller.getLeftY() * 0.15 + 0.05; - IntakerSubsystem.setIntakerRotater(rotaterspeed); + } else if(controller.getLeftY() > 0.5){ + IntakerSubsystem.IntakerDownNonStop(); + // double rotaterspeed = -controller.getLeftY() * 0.3 + 0.2; + // 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 a6cf917..9434e3f 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -40,16 +40,16 @@ public class ShooterControls { case PassingBall: 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 = -1.4964026859575532*xP*xP*xP*xP*xP*xP*xP+29.82445640241506*xP*xP*xP*xP*xP*xP-247.71603153159896*xP*xP*xP*xP*xP+1108.8727302184625*xP*xP*xP*xP-2881.517500617481*xP*xP*xP+4332.05363145275*xP*xP-3467.2508337871454*xP+1192.1508062663759; flywheelSpeed += flywheelSpeedOffset; - backboardPosition = xP*xP;//todo:实测传球的时候的背板位置 打表 - 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; + backboardPosition = +34.5286998307899*xP*xP*xP*xP*xP*xP*xP*xP-788.5788623683884*xP*xP*xP*xP*xP*xP*xP+7658.073303693148*xP*xP*xP*xP*xP*xP-41191.69338972519*xP*xP*xP*xP*xP+133870.54588688645*xP*xP*xP*xP-268559.6987062158*xP*xP*xP+324254.6530324262*xP*xP-215015.48893131423*xP+59977.46545382827; + backboardPosition += backboardPositionOffset; conveyorSpeed = Constants.Shooter.conveyorSpeed; if (controller.getRightTriggerAxis() < 0.1) { flywheelSpeed = 0; conveyorSpeed = 0; } - if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() { diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index ddaf147..04f7a86 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -97,7 +97,7 @@ public class IntakerSubsystem extends SubsystemBase{ double currentPosition = getIntakerotaterPosition(); if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) { // move up while below the upper limit - intakerotater.set(0.1); + intakerotater.set(0.2); } else { // at or above upper limit: stop stopIntakerotater();