From cae3810ef5d4eeb7123b13bd92b6fcdf3e613c26 Mon Sep 17 00:00:00 2001
From: OnDYCLaptop <dingdoorfiah@gamil.com>
Date: Tue, 3 Mar 2026 14:40:21 +0800
Subject: [PATCH] autocommand

---
 src/main/java/frc/robot/Constants.java        |  2 +-
 .../java/frc/robot/controls/Autocommand.java  | 26 +++++++++++++++++++
 .../frc/robot/controls/IntakerControls.java   |  2 +-
 .../frc/robot/controls/ShooterControls.java   |  1 -
 4 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7ec47e8..f149667 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -111,7 +111,7 @@ public final class Constants {
         .withKV(0.00).withKA(0.00).withKS(0.00);
     //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
 
-    public static final double IntakeGetterSpeed = 0.0;
+    public static final double IntakeGetterSpeedforAuto = 0.0;
     // 单位RPS
     public static final double positionToleranceRotations = 0.00;
     //rotater容差
diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java
index e186ed7..afe72ec 100644
--- a/src/main/java/frc/robot/controls/Autocommand.java
+++ b/src/main/java/frc/robot/controls/Autocommand.java
@@ -17,6 +17,7 @@ import frc.robot.Constants;
 import frc.robot.Constants.RobotStatus;
 
     public class Autocommand extends SequentialCommandGroup{
+        
         public static Command climb_up(ClimberSubsystem climber) {
             return new RunCommand(() -> {
                 climber.climbUp();  // 控制爬升器上升
@@ -33,6 +34,31 @@ import frc.robot.Constants.RobotStatus;
                 climber.holdPosition();  // 停止并锁定当前爬升器位置
             }, climber));
         }
+        public static Command start_intake(IntakerSubsystem intaker) {
+            return new InstantCommand(() -> {
+                intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto);  // 启动吸球
+            }, intaker);
+        }
+        public static Command stop_intake(IntakerSubsystem intaker) {
+            return new InstantCommand(() -> {
+                intaker.setIntakerGetterSpeed(0);  // 停止吸球
+            }, intaker);
+        }
+        public static Command intaker_down(IntakerSubsystem intaker) {
+            return new InstantCommand(() -> {
+                intaker.IntakerDownNonStop();  // 吸球机构下降
+            }, intaker).withTimeout(0.5).andThen(new InstantCommand(() -> {
+                intaker.stopIntakerotater();;  // 吸球机构保持当前位置
+            }, intaker));
+        }
+        public static Command intaker_up(IntakerSubsystem intaker) {
+            return new InstantCommand(() -> {
+                intaker.IntakerUpNonStop();  // 吸球机构上升
+            }, intaker).withTimeout(0.5).andThen(new InstantCommand(() -> {
+                intaker.stopIntakerotater();;  // 吸球机构保持当前位置
+            }, intaker));
+        }
+        
     }
 
 
diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java
index 4b13fcc..b93de5b 100644
--- a/src/main/java/frc/robot/controls/IntakerControls.java
+++ b/src/main/java/frc/robot/controls/IntakerControls.java
@@ -74,7 +74,7 @@ public class IntakerControls {
             getterspeed = 0;
             }
         IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
-
+        
         if(controller.getLeftY() < -0.5){
                     IntakerSubsystem.IntakerDownNonStop();
             } else if(controller.getLeftY() > 0.5){
diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java
index 9d52b49..4d2ae42 100644
--- a/src/main/java/frc/robot/controls/ShooterControls.java
+++ b/src/main/java/frc/robot/controls/ShooterControls.java
@@ -67,7 +67,6 @@ public class ShooterControls {
                     }  
                     break;
                 case AutoAimming:
-                
                     var x = shooterSubsystem.getDistance();
                     x = Math.max(1.2, Math.min(4.5, x));
                     flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;