diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 588f5ae..c6bfa7c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 final double climbDownTicks = 0; // TODO: 实测后调整 - public static final double climbDownVelocity = -200; + public static double climbUpTicks = 94; // TODO: 实测后调整 + public static double climbDownTicks = 0; // TODO: 实测后调整 + public static final double climbVelocity = -200; public static final double holdToleranceRot = 0.5; public static final double deltaDownTicks = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e6559de..76057f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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)); } } diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index 29a78ce..e186ed7 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/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)); + } + } + + + + diff --git a/src/main/java/frc/robot/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java index 2827ba2..543aff9 100644 --- a/src/main/java/frc/robot/controls/ClimberControls.java +++ b/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.y().getAsBoolean()&&controller.x().getAsBoolean()){ + if(controller.getRightY() < -0.5){ climber.climbDownNonStop(); - } - else if (controller.y().getAsBoolean()) { + } else if(controller.getRightY() > 0.5){ + 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); + } } diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index 21beeda..32c29ec 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/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) { - this.intakerSubsystem = intakerSubsystem; + public IntakerControls(IntakerSubsystem Intaker, CommandXboxController controller) { + this.IntakerSubsystem = Intaker; this.controller = controller; } @@ -73,8 +73,18 @@ public class IntakerControls { if (controller.getLeftTriggerAxis() < 0.1) { getterspeed = 0; } - intakerSubsystem.setIntakerGetterSpeed(getterspeed); - - }, intakerSubsystem); + IntakerSubsystem.setIntakerGetterSpeed(getterspeed); + + 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); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index dc2e387..9d52b49 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index f054a7f..855bf5f 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index 92632a3..644e14c 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/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() { - intakerotater.setControl(velocityRequest.withVelocity(0)); + public void stopIntakerotater() { + intakerotater.set(0); // 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI） } //intakegetter的on/off - public void setIntakeGetterOn(boolean on) { - if (on) { - intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed)); - } - else { - intakegetter.setControl(velocityRequest.withVelocity(0)); - } - } + // public void setIntakeGetterOn(boolean on) { + // if (on) { + // intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed)); + // } + // else { + // 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(); + } + } }