this.hood = hood;
this.intake = intake;
this.spindexer = spindexer;
- this.climb = climb;
}
- public void configureControls() {
- // Reset the yaw. Mainly useful for testing/driver practice
- controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
-
- // Cancel commands
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
-
- // Reverse motors
- if (intake != null && spindexer != null) {
- controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
- }
-
- // Intake
- if (intake != null) {
- // Toggle intake
- controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.extend();
- intake.spinStart();
- intakeBoolean = false;
- } else {
- intake.intermediateExtend();
- intake.spinStop();
- intakeBoolean = true;
- }
- }, intake));
-
- // Retract if hold for 2 seconds
- controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
- intake.retract();
- intakeBoolean = true;
- intake.spinStop();
- }, intake));
-
- // Make the intake go in and out while shooting
- controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
- .alongWith(new InstantCommand(()-> intakeBoolean = true)));
-
- // Calibration: you can now calibrate easily using this button
- if (hood != null && intake != null) {
- controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- hood.calibrate();
- }, intake, hood)).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- hood.stopCalibrating();
- }, intake, hood));
- }
-
- // Stop intake roller
- controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{
- if(intakeBoolean){
- intake.spinStart();
- intakeBoolean = false;
- } else{
- intake.spinStop();
- intakeBoolean = true;
- }
- }));
- }
-
- // Spindexer
- if (spindexer != null && turret != null && hood != null && intake != null) {
-
- // Toggle spindexer
- controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
- new RunSpindexer(spindexer, turret, hood, intake)
- );
- }
-
- // Auto shoot
- if (turret != null && hood != null && shooter != null && spindexer != null) {
- autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
- controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
- }
-
-
- // Hood
- if (hood != null) {
- // Set the hood down -- for safety measures under trench
- controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{
- hood.forceHoodDown(true);
- }, hood)).onFalse(new InstantCommand(()->{
- hood.forceHoodDown(false);
- }));
- }
-
- // shoot focus mode: reduces drive current
- if (spindexer != null) {
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
- .onFalse(new InstantCommand(()-> shootFocus(false)));
- }
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Reverse motors
+ if (intake != null && spindexer != null) {
+ controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
}
- private void shootFocus(boolean turnOn) {
- if (turnOn) {
- System.out.println("Shooting is now Focused");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25);
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
} else {
- System.out.println("Shooting back to normal (From focus)");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
}
+ }, intake));
+
+ // Retract if hold for 2 seconds
+ controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ intake.spinStop();
+ }, intake));
+
+ // Make the intake go in and out while shooting
+ controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
+ .alongWith(new InstantCommand(() -> intakeBoolean = true)));
+
+ // Calibration: you can now calibrate easily using this button
+ if (hood != null && intake != null) {
+ controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+ intake.calibrate();
+ hood.calibrate();
+ }, intake, hood)).onFalse(new InstantCommand(() -> {
+ intake.stopCalibrating();
+ hood.stopCalibrating();
+ }, intake, hood));
+ }
+
+ // Stop intake roller
+ controller.get(DPad.DOWN).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }));
}
- @Override
- public double getRawSideTranslation() {
- return controller.get(PS5Axis.LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(PS5Axis.LEFT_Y);
- }
+ // Spindexer
+ if (spindexer != null && turret != null && hood != null && intake != null) {
- @Override
- public double getRawRotation() {
- return controller.get(PS5Axis.RIGHT_X);
+ // Toggle spindexer
+ controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
+ new RunSpindexer(spindexer, turret, hood, intake));
}
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null && spindexer != null) {
+ autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
+ controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
}
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
+ // Hood
+ if (hood != null) {
+ // Set the hood down -- for safety measures under trench
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
+ hood.forceHoodDown(true);
+ }, hood)).onFalse(new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }));
}
- // Hood
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
++ // shoot focus mode: reduces drive current
+ if (spindexer != null) {
- // Set the hood down -- for safety measures under trench
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
+ .onFalse(new InstantCommand(() -> shootFocus(false)));
}
-
- @Override
- public boolean getIsAlign() {
- return false;
+ }
+
+ private void shootFocus(boolean turnOn) {
+ if (turnOn) {
+ System.out.println("Shooting is now Focused");
+ spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
+ drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
+ DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25);
+ } else {
+ System.out.println("Shooting back to normal (From focus)");
+ spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
+ drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
}
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(PS5Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(PS5Axis.LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(PS5Axis.RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
}