From c0ae545aac0db13d64192f6e23561634ce66a70a Mon Sep 17 00:00:00 2001 From: Heggiehog Date: Sun, 13 Oct 2024 10:35:39 -0700 Subject: [PATCH] Intake backup off rotations instead of seconds (untested) --- .../java/frc/robot/commands/AdvancedIntakeCommand.java | 4 +--- .../java/frc/robot/commands/SerializerBackupCommand.java | 8 ++++++-- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++++ 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java index af630e0..802e882 100644 --- a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java +++ b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java @@ -18,8 +18,6 @@ public AdvancedIntakeCommand( addCommands( new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem), new ParallelCommandGroup( - new SerializerBackupCommand(shooterSubsystem) - .withTimeout(0.04) - .andThen(new StopShooterCommand(shooterSubsystem)))); + new SerializerBackupCommand(shooterSubsystem))); } } diff --git a/src/main/java/frc/robot/commands/SerializerBackupCommand.java b/src/main/java/frc/robot/commands/SerializerBackupCommand.java index d8a2e2a..68c6a07 100644 --- a/src/main/java/frc/robot/commands/SerializerBackupCommand.java +++ b/src/main/java/frc/robot/commands/SerializerBackupCommand.java @@ -11,6 +11,7 @@ public class SerializerBackupCommand extends Command { /** Creates a new SerializerBackupCommand. */ ShooterSubsystem shooterSubsystem; + double serializerPos; public SerializerBackupCommand(ShooterSubsystem shooterSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.shooterSubsystem = shooterSubsystem; @@ -21,6 +22,7 @@ public SerializerBackupCommand(ShooterSubsystem shooterSubsystem) { @Override public void initialize() { shooterSubsystem.setShooterMode(ShooterMode.SERIALIZER_BACKUP); + serializerPos = shooterSubsystem.getSerializerPositon(); } // Called every time the scheduler runs while the command is scheduled. @@ -29,11 +31,13 @@ public void execute() {} // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + shooterSubsystem.setShooterMode(ShooterMode.IDLE); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return serializerPos - 0.5 > shooterSubsystem.getSerializerPositon(); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 884669e..0bc3fb0 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -156,6 +156,10 @@ public ShooterMode getMode() { return shooterMode; } + public double getSerializerPositon(){ + return serializerMotor.getRotorPosition().getValueAsDouble(); + } + public boolean isShooterUpToSpeed() { return pivotShooterTopMotor.getVelocity().getValueAsDouble() >= Shooter.SHOOTER_VELOCITY_THRESHOLD