手动完成版（待测试）

src/main/java/frc/robot/Constants.java

@@ -98,6 +98,7 @@ public final class Constants {
 
     public static final double intakeRotaterUpLimit = 0.0;  // rotator上限角度
     public static final double intakeRotaterDownLimit = 0.0;  // rotator下限角度
+    public static final double intakeRotaterVelocity = 10; // rotater的速度（如果你用速度控制的话）
 
     public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs()
         .withKP(0.00).withKI(0.00).withKD(0.00)
@@ -122,9 +123,9 @@ public final class Constants {
 
     public static final boolean inverted = false;
 
-    public static final double climbUpTicks = 94; // TODO: 实测后调整
+    public static double climbUpTicks = 94; // TODO: 实测后调整
-    public static final double climbDownTicks = 0; // TODO: 实测后调整
+    public static double climbDownTicks = 0; // TODO: 实测后调整
-    public static final double climbDownVelocity = -200;
+    public static final double climbVelocity = -200;
     public static final double holdToleranceRot = 0.5; 
     public static final double deltaDownTicks = 1;
 

src/main/java/frc/robot/RobotContainer.java

@@ -160,10 +160,14 @@ public class RobotContainer {
   }
 
   private void configueClimber() {
-    
     enableTrigger.onTrue(climberControls.enableInitCommand());
-
     climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
+    controllerupper.b().onTrue(climberControls.resetClimberEncoderCommand(climberSubsystem));
+  }
+
+  private void configueIntaker() {
+    IntakerSubsystem.setDefaultCommand(intakerControls.defaultIntakerCommand());
+    controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
   }
 
 }

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

@@ -4,7 +4,10 @@ import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.RobotStatusManager;
 import frc.robot.subsystems.IntakerSubsystem;
@@ -13,5 +16,27 @@ import frc.robot.subsystems.CommandSwerveDrivetrain;
 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();  // 控制爬升器上升
+            }, climber).withTimeout(1.0)  // 
+            .andThen(new InstantCommand(() -> {
+                climber.holdPosition();  // 停止并锁定当前爬升器位置
+            }, climber));
+        }
+        public static Command climb_down(ClimberSubsystem climber) {
+            return new RunCommand(() -> {
+                climber.climbDown();  // 控制爬升器下降
+            }, climber).withTimeout(1.0)  //
+            .andThen(new InstantCommand(() -> {
+                climber.holdPosition();  // 停止并锁定当前爬升器位置
+            }, climber));
+        }
+    }
+
+
+
+
 
 

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

@@ -23,11 +23,11 @@ public class ClimberControls {
     public Command defaultClimberCommand() {
         return Commands.run(()->{
             //climber.climbUp();
-                // 优先级：如果同时按，优先上升（你也可以反过来）
+                if(controller.getRightY() < -0.5){
-                if(controller.y().getAsBoolean()&&controller.x().getAsBoolean()){
                     climber.climbDownNonStop();
-                }
+                } else if(controller.getRightY() > 0.5){
-                else if (controller.y().getAsBoolean()) {
+                    climber.climbUpNonStop();
+                } else if (controller.y().getAsBoolean()) {
                     climber.climbUp();
                 } else if (controller.x().getAsBoolean()) {
                     climber.climbDown();
@@ -50,4 +50,7 @@ public class ClimberControls {
                 .withTimeout(0.5)
                 .andThen(Commands.runOnce(climber::zeroEncoder, climber));
     }
+    public Command resetClimberEncoderCommand(ClimberSubsystem climber) {
+        return Commands.runOnce(climber::zeroEncoder, climber);
+    }
 }

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

@@ -10,13 +10,13 @@ import frc.robot.Constants;
 
 
 public class IntakerControls {
-    private final IntakerSubsystem intakerSubsystem;
+    private final IntakerSubsystem IntakerSubsystem;
     private final CommandXboxController controller;
 
-    private static final double ROTATER_STEP_PER_CYCLE = 0.00; // 需要你按实际速度调：越大越快（fromAI）
+    private static final double ROTATER_STEP_PER_CYCLE = 0.00;
 
-    public IntakerControls(IntakerSubsystem intakerSubsystem, CommandXboxController controller) {
+    public IntakerControls(IntakerSubsystem Intaker, CommandXboxController controller) {
-        this.intakerSubsystem = intakerSubsystem;
+        this.IntakerSubsystem = Intaker;
         this.controller = controller;
     }
 
@@ -73,8 +73,18 @@ public class IntakerControls {
         if (controller.getLeftTriggerAxis() < 0.1) {
             getterspeed = 0;
             }
-        intakerSubsystem.setIntakerGetterSpeed(getterspeed);
+        IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
 
-        }, intakerSubsystem);
+        if(controller.getLeftY() < -0.5){
+                    IntakerSubsystem.IntakerDownNonStop();
+            } else if(controller.getLeftY() > 0.5){
+                    IntakerSubsystem.IntakerUpNonStop();
+            } else {
+                    IntakerSubsystem.stopIntakerotater();
+            }
+        }, IntakerSubsystem);
+    }
+    public Command resetIntakerRotaterEncoderCommand(IntakerSubsystem intakerSubsystem) {
+        return Commands.runOnce(intakerSubsystem::zeroEncoder, intakerSubsystem);
     }
 }

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

@@ -40,7 +40,6 @@ public class ShooterControls {
                     var xP = shooterSubsystem.getDistance();
                     //xP = Math.max(1.2, Math.min(4.5, xP));
                     flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351;
-                    
                     flywheelSpeed += flywheelSpeedOffset;
                     backboardPosition = xP*xP;//todo:实测传球的时候的背板位置 打表
                     backboardPosition += backboardPositionOffset;

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -70,10 +70,14 @@ public class ClimberSubsystem extends SubsystemBase {
     public void climbDown() {
         targetPositionTicks = Constants.Climber.climbDownTicks;  
         climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
-        
     }
+
     public void climbDownNonStop(){
-        climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbDownVelocity));
+        climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
+    }
+    
+    public void climbUpNonStop(){
+        climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
     }
 
     public void captureHoldPosition() {

src/main/java/frc/robot/subsystems/IntakerSubsystem.java

@@ -50,21 +50,41 @@ public class IntakerSubsystem extends SubsystemBase{
         return error <= Constants.Intaker.positionToleranceRotations;
     }
 
-     public void stopintakerotater() {
+     public void stopIntakerotater() {
-        intakerotater.setControl(velocityRequest.withVelocity(0));
+        intakerotater.set(0);
         // 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
     }
 
     //intakegetter的on/off
-    public void setIntakeGetterOn(boolean on) {
+    // public void setIntakeGetterOn(boolean on) {
-        if (on) {
+    //     if (on) {
-            intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
+    //         intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
-        }
+    //     }
-        else {
+    //     else {
-            intakegetter.setControl(velocityRequest.withVelocity(0));
+    //         intakegetter.setControl(velocityRequest.withVelocity(0));
-        }
+    //     }
-    }
+    // }
     public void setIntakerGetterSpeed(double speed) {
         intakegetter.setControl(velocityRequest.withVelocity(speed));
     }
+    public void zeroEncoder(){
+        intakerotater.setPosition(0);
+        stopIntakerotater();
+    }
+    public void IntakerDownNonStop(){
+        double currentPosition = getIntakerotaterPosition();
+        if (currentPosition < Constants.Intaker.intakeRotaterDownLimit) {
+            intakerotater.setControl(velocityRequest.withVelocity(Constants.Intaker.intakeRotaterVelocity));
+        } else {
+            stopIntakerotater();
+        }
+    }
+    public void IntakerUpNonStop(){
+        double currentPosition = getIntakerotaterPosition();
+        if (currentPosition > Constants.Intaker.intakeRotaterUpLimit) {
+            intakerotater.setControl(velocityRequest.withVelocity(-Constants.Intaker.intakeRotaterVelocity));
+        } else {
+            stopIntakerotater();
+        }
+    }
 }