autocommand

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;

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_", idle(superstructure));
-        NamedCommands.registerCommand("stop_intake_and_conveyor", idle(superstructure));
-        NamedCommands.registerCommand("start_intake_timelimit", timedIntake(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 autoAimForShoot(DriveControls driveControls) {
-    public static Command autoAim(DriveControls driveControls) {
+        return driveControls.autoAimForShootCommand();
-        return driveControls.autoAimCommand();
     }
 
-    public static Command shake(DriveControls driveControls) {
-        return driveControls.shakeCommand();
-    }
 }

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[] hubAim = calculateDistanceAndRotationToHub();
-        double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
+            double distanceToHub = hubAim[0];
-        return fieldCentric
+            double angleDifferenceToHub = hubAim[1];
-            .withVelocityX(0.0)
+
-            .withVelocityY(0.0)
+            autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceToHub);
-            .withRotationalRate(vomega);
+            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(Constants.Superstructure.shootTimeoutSeconds)
-        .withTimeout(1.0)
+        .andThen(new InstantCommand(() -> drivetrain.stop(), drivetrain));
-        .andThen(new InstantCommand(() -> {
-            drivetrain.stop();
-        }, drivetrain));
     }
+
     public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
     Timer timer = new Timer();