Skip to content

Commit

Permalink
Intake backup off rotations instead of seconds (untested)
Browse files Browse the repository at this point in the history
  • Loading branch information
NoraZitnick committed Oct 13, 2024
1 parent 646f21a commit c0ae545
Show file tree
Hide file tree
Showing 3 changed files with 11 additions and 5 deletions.
4 changes: 1 addition & 3 deletions src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -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)));
}
}
8 changes: 6 additions & 2 deletions src/main/java/frc/robot/commands/SerializerBackupCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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.
Expand All @@ -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();
}
}
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit c0ae545

Please sign in to comment.