添加自动射球时的shake

src/main/java/frc/robot/controls/Autocommand.java

@@ -40,7 +40,10 @@ import frc.robot.Constants.RobotStatus;
         NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
         NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
         NamedCommands.registerCommand("shoot", shoot(shooter));
-        NamedCommands.registerCommand("auto_aim_and_shoot", autoAimAndShoot(driveControls, shooterControls));
+        NamedCommands.registerCommand("Shake", Autocommand.shake(driveControls));
+        NamedCommands.registerCommand("AutoAim", Autocommand.autoAim(driveControls));
+        NamedCommands.registerCommand("AutoShoot", Autocommand.autoShoot(shooterControls));
+
     }
 
         public static Command climb_up(ClimberSubsystem climber) {
@@ -131,10 +134,14 @@ import frc.robot.Constants.RobotStatus;
                 shooter.setConveyorSpeedByRPS(0);  // 停止输送
             }, shooter)));
         }
-        
-        public static Command autoAimAndShoot(DriveControls driveControls, ShooterControls shooterControl) {
-            return driveControls.autoAimCommand()
-            .andThen(shooterControl.autoShootToHubCommand());
+        public static Command autoAim(DriveControls driveControls) {
+            return driveControls.autoAimCommand();
+        }
+        public static Command autoShoot(ShooterControls shooterControl) {
+            return shooterControl.autoShootToHubCommand();
+        }
+        public static Command shake(DriveControls driveControls) {
+            return driveControls.shakeCommand();
         }
 
 

src/main/java/frc/robot/controls/DriveControls.java

@@ -25,6 +25,7 @@ import frc.robot.subsystems.LimelightSupplier;
 import java.util.Optional;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer; 
 
 public class DriveControls {
 
@@ -430,7 +431,29 @@ public class DriveControls {
             drivetrain.stop();
         }, drivetrain));
     }
+    public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
+        Timer timer = new Timer();
 
-
+        return Commands.sequence(
+            new InstantCommand(timer::restart),
+            drivetrain.applyRequest(() -> {
+                double t = timer.get();
+                boolean positive = ((int) (t / switchPeriod)) % 2 == 0;
+                double vy = positive ? amplitude : -amplitude;
+                return robotCentric
+                    .withVelocityX(0.0)
+                    .withVelocityY(vy)
+                    .withRotationalRate(0.0);
+            }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime),
+            new InstantCommand(() -> {
+                timer.stop();
+                autoControlNetworkTable.getEntry("ShakeVy").setDouble(0.0);
+                drivetrain.stop();
+            }, drivetrain)
+        );
+    }
+    public Command shakeCommand() {
+        return shakeCommand(0.25, 0.2, 3.0);
+    }
     
 }