From 9e00148138784befa6f438c260ab90a1d7ce73e7 Mon Sep 17 00:00:00 2001
From: OnDYCLaptop <dingdoorfiah@gamil.com>
Date: Mon, 9 Mar 2026 12:27:28 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=87=AA=E5=8A=A8=E5=B0=84?=
 =?UTF-8?q?=E7=90=83=E6=97=B6=E7=9A=84shake?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../java/frc/robot/controls/Autocommand.java  | 17 +++++++----
 .../frc/robot/controls/DriveControls.java     | 29 +++++++++++++++++--
 2 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java
index e3b194b..46f83c8 100644
--- a/src/main/java/frc/robot/controls/Autocommand.java
+++ b/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();
         }
 
 
diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java
index 4e00c21..a73a46e 100644
--- a/src/main/java/frc/robot/controls/DriveControls.java
+++ b/src/main/java/frc/robot/controls/DriveControls.java
@@ -24,7 +24,8 @@ 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.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);
+    }
+    
 }