From 632da8d6549705d921a06e6031a09b6c62ddd184 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Sat, 11 Jul 2026 23:01:27 +0800 Subject: [PATCH] autocommand --- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/controls/Autocommand.java | 29 ++------------ .../frc/robot/controls/DriveControls.java | 40 +++++++++++++------ 3 files changed, 33 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4fc8832..dde2be9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -158,7 +158,7 @@ public final class Constants { public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity; public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity; - public static final double shootTimeoutSeconds = 1.0; + public static final double shootTimeoutSeconds = 3.0; public static final double aimToleranceDegrees = 1.5; public static final double stationarySpeedToleranceMetersPerSecond = 0.5; public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage; diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index 2b37f66..fc262cc 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -23,29 +23,17 @@ public class Autocommand extends SequentialCommandGroup { OperatorControls operatorControls ) { NamedCommands.registerCommand("start_intake", startIntake(superstructure)); - NamedCommands.registerCommand("start_intake_and_conveyor", startIntake(superstructure)); - NamedCommands.registerCommand("stop_intake_and_conveyor", idle(superstructure)); - NamedCommands.registerCommand("start_intake_timelimit", timedIntake(superstructure)); - NamedCommands.registerCommand("stop_intake", idle(superstructure)); + NamedCommands.registerCommand("stop_intake_", idle(superstructure)); NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater)); - NamedCommands.registerCommand("intaker_up", raiseIntaker(intakeRotater)); NamedCommands.registerCommand("shoot_hub", shootHub(superstructure)); - NamedCommands.registerCommand("Shake", shake(driveControls)); - NamedCommands.registerCommand("AutoAim", autoAim(driveControls)); NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command()); + NamedCommands.registerCommand("AutoAimForShoot", autoAimForShoot(driveControls)); } public static Command startIntake(Superstructure superstructure) { return superstructure.requestIntakeCommand(); } - public static Command timedIntake(Superstructure superstructure) { - return Commands.sequence( - superstructure.requestIntakeCommand(), - Commands.waitSeconds(1.5), - superstructure.requestIdleCommand() - ); - } public static Command idle(Superstructure superstructure) { return superstructure.requestIdleCommand(); } @@ -54,11 +42,6 @@ public class Autocommand extends SequentialCommandGroup { return intakeRotater.lowerForMatchCommand(); } - public static Command raiseIntaker(IntakeRotaterSubsystem intakeRotater) { - return intakeRotater.raiseForMatchCommand(); - } - - // 自动射击和手动射击使用同一套 gated shooting 流程。 public static Command shootHub(Superstructure superstructure) { return Commands.sequence( superstructure.requestShootHubCommand(), @@ -66,12 +49,8 @@ public class Autocommand extends SequentialCommandGroup { superstructure.requestIdleCommand() ); } - - public static Command autoAim(DriveControls driveControls) { - return driveControls.autoAimCommand(); + public static Command autoAimForShoot(DriveControls driveControls) { + return driveControls.autoAimForShootCommand(); } - public static Command shake(DriveControls driveControls) { - return driveControls.shakeCommand(); - } } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index af1ea8a..fac25d3 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -338,21 +338,37 @@ public class DriveControls { public Command getAllianceColorCommand() { return Commands.runOnce(this::setTeamColors); } - public Command autoAimCommand() { + + // DriveControls.java + public Command autoAimForShootCommand() { return drivetrain.applyRequest(() -> { - double[] distanceAndRotation = calculateDistanceAndRotationToHub(); - double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); - return fieldCentric - .withVelocityX(0.0) - .withVelocityY(0.0) - .withRotationalRate(vomega); + double[] hubAim = calculateDistanceAndRotationToHub(); + double distanceToHub = hubAim[0]; + double angleDifferenceToHub = hubAim[1]; + + autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceToHub); + autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(angleDifferenceToHub); + + drivetrain.setTargetHeading( + drivetrain.getRotation().plus(Rotation2d.fromDegrees(angleDifferenceToHub)) + ); + + double vomega = calculateRotationSpeedFromRotationAngle(angleDifferenceToHub); + + autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); + autoControlNetworkTable.getEntry("ActualVX").setDouble(0.0); + autoControlNetworkTable.getEntry("ActualVY").setDouble(0.0); + autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega); + + return fieldCentric + .withVelocityX(0.0) + .withVelocityY(0.0) + .withRotationalRate(vomega); }, () -> DriveMode.FIELD_CENTRIC) - .until(() -> Math.abs(calculateDistanceAndRotationToHub()[1]) < 1.5) - .withTimeout(1.0) - .andThen(new InstantCommand(() -> { - drivetrain.stop(); - }, drivetrain)); + .withTimeout(Constants.Superstructure.shootTimeoutSeconds) + .andThen(new InstantCommand(() -> drivetrain.stop(), drivetrain)); } + public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) { Timer timer = new Timer();