drive);
}
+ private boolean seizing;
+
public void registerCommands() {
- if (intake != null){
- NamedCommands.registerCommand("Extend Intake", new InstantCommand(()-> intake.extend()));
- NamedCommands.registerCommand("Retract Intake", new InstantCommand(()-> intake.retract()));
+ if (intake != null) {
+ NamedCommands.registerCommand("Extend Intake", new InstantCommand(() -> intake.extend()));
+ NamedCommands.registerCommand("Retract Intake", new InstantCommand(() -> intake.retract()));
+ NamedCommands.registerCommand("Intermediate Extend", new InstantCommand(() -> intake.intermediateExtend()));
+ NamedCommands.registerCommand("Spin Intake Rollers", new InstantCommand(() -> intake.spinStart()));
+ NamedCommands.registerCommand("Stop Intake Rollers", new InstantCommand(() -> intake.spinStop()));
+
+ NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(() -> {
+ seizing = true;
+ CommandScheduler.getInstance().schedule(new IntakeMovementCommand(intake).until(() -> !seizing));
+ }));
+ NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(() -> {
+ seizing = false;
+ }));
}
- if (turret != null && drive != null && hood != null && shooter != null && spindexer != null) {
+ if (intake != null && spindexer != null){
+ NamedCommands.registerCommand("Spin Intake Rollers", new ParallelCommandGroup(
+ new InstantCommand(()->intake.spin(IntakeConstants.SPEED))
+ ));
+ NamedCommands.registerCommand("Stop Intake Rollers", new ParallelCommandGroup(
+ new InstantCommand(()->intake.spinStop())
+ ));
+ Command intakeMovement = new IntakeMovementCommand(intake);
+ NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(()-> intakeMovement.schedule()));
+ NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(()-> intakeMovement.cancel()));
+ }
+
+ if (turret != null && drive != null && hood != null && shooter != null && spindexer != null){
+ Command runSpindexer = new RunSpindexer(spindexer, turret);
NamedCommands.registerCommand("Auto shoot", new AutoShootCommand(turret, drive, hood, shooter, spindexer));
- NamedCommands.registerCommand("Start Spindexer", new InstantCommand(() -> spindexer.maxSpindexer(), spindexer));
- NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> spindexer.stopSpindexer()));
+ NamedCommands.registerCommand("Start Spindexer", new InstantCommand(() -> runSpindexer.schedule()));
+ NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> runSpindexer.cancel()));
}
- if (hood != null){
- NamedCommands.registerCommand("Hood Down", new InstantCommand(()->{hood.forceHoodDown(true);}));
- NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(()-> {hood.forceHoodDown(false);}));
+ if (hood != null) {
+ NamedCommands.registerCommand("Hood Down", new InstantCommand(() -> {
+ hood.forceHoodDown(true);
+ }));
+ NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }));
}
-
- if (linearClimb != null && drive != null){
+ if (linearClimb != null && drive != null) {
NamedCommands.registerCommand("Climb", new ClimbDriveCommand(linearClimb, drive));
}