* Driver controls for the PS5 controller
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
- public final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
- private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
++ private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
private final BooleanSupplier slowModeSupplier = () -> false;
private boolean intakeBoolean = true;
private Command autoShoot = null;
new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
- driver.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
getDrivetrain().setIsAlign(false);
getDrivetrain().setDesiredPose(() -> null);
CommandScheduler.getInstance().cancelAll();
interrupted -> getDrivetrain().setStateDeadband(true),
() -> false, getDrivetrain()).withTimeout(2));
- controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
- .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false)));
+ // Trench align
- driver.get(DPad.LEFT).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
++ controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
+ .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false)));
- // Trench assist
- driver.get(DPad.RIGHT).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true)))
- .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false)));
+ controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(true); getDrivetrain().setTrenchAlign(true);}))
+ .onFalse(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(false); getDrivetrain().setTrenchAlign(false);}));
+
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(true); getDrivetrain().setTrenchAlign(true);}))
+ .onFalse(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(false); getDrivetrain().setTrenchAlign(false);}));
-
-
-
- // // Intake
- // if (intake != null) {
- // driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
- // if (intakeBoolean) {
- // intake.extend();
- // intake.spinStart();
- // intakeBoolean = false;
- // } else {
- // intake.intermediateExtend();
- // intake.spinStop();
- // intakeBoolean = true;
- // }
- // }));
-
- // // Retract if hold for 3 seconds
- // driver.get(PS5Button.CROSS).debounce(3.0).onTrue(new InstantCommand(()->{
- // intake.retract();
- // intakeBoolean = true;
- // }));
-
- // // Calibration
- // driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(()->{
- // intake.calibrate();
- // })).onFalse(new InstantCommand(()->{
- // intake.stopCalibrating();
- // }));
- // }
-
- // // Spindexer
- // if (spindexer != null){
- // // Will only run if we are not calling default shoot command
- // driver.get(PS5Button.LB).onTrue(new InstantCommand(()-> spindexer.maxSpindexer()))
- // .onFalse(new InstantCommand(()-> spindexer.stopSpindexer()));
- // }
-
- // // Auto shoot
- // if (turret != null) {
- // driver.get(PS5Button.SQUARE).onTrue(
- // new InstantCommand(() -> {
- // if (autoShoot != null && autoShoot.isScheduled()) {
- // autoShoot.cancel();
- // } else {
- // autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
- // CommandScheduler.getInstance().schedule(autoShoot);
- // }
- // }));
- // }
-
- // if (climb != null) {
- // driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
- // climb.hardstopCalibration();
- // })).onFalse(new InstantCommand(() -> {
- // climb.stopCalibrating();
- // }));
- // }
+ // Reverse motors
+ if (intake != null && spindexer != null && shooter != null) {
- driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
+ reverseMotors = new ReverseMotors(intake, spindexer);
+ reverseMotors.schedule();
+ })).onFalse(new InstantCommand(() -> {
+ if (reverseMotors != null) {
+ reverseMotors.cancel();
+ }
+ }));
+ }
+
+ // Intake
+ if (intake != null) {
+ // Toggle intake
- driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }));
+
+ // Retract if hold for 3 seconds
- driver.get(PS5Button.RIGHT_TRIGGER).debounce(3.0).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.RIGHT_TRIGGER).debounce(3.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ }));
+
+ // Calibration
- driver.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+ intake.calibrate();
+ })).onFalse(new InstantCommand(() -> {
+ intake.stopCalibrating();
+ }));
+ }
+
+ // Spindexer
+ if (spindexer != null) {
+ // Will only run if we are not calling default shoot command
- driver.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
++ controller.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
+ .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
+ }
+
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null) {
- driver.get(PS5Button.SQUARE).onTrue(
++ controller.get(PS5Button.SQUARE).onTrue(
+ new InstantCommand(() -> {
+ if (autoShoot != null && autoShoot.isScheduled()) {
+ autoShoot.cancel();
+ } else {
+ autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
+ CommandScheduler.getInstance().schedule(autoShoot);
+ }
+ }));
+ }
+
+ // Climb
+ if (climb != null) {
+ // Calibration
- driver.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+ climb.hardstopCalibration();
+ })).onFalse(new InstantCommand(() -> {
+ climb.stopCalibrating();
+ }));
+
+ // Climb retract
- driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
+ climb.retract();
+ }));
+
+ // Drive to climb position and rumble
- driver.get(PS5Button.TRIANGLE).onTrue(new SequentialCommandGroup(
++ controller.get(PS5Button.TRIANGLE).onTrue(new SequentialCommandGroup(
+ new ClimbDriveCommand(climb, getDrivetrain()),
+ new InstantCommand(() -> this.startRumble()),
+ new WaitCommand(1),
+ new InstantCommand(() -> this.endRumble())));
+ }
+
+ // Hood
+ if (hood != null) {
+ // Calibration
- driver.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
++ controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+ hood.calibrate();
+ })).onFalse(new InstantCommand(() -> {
+ hood.stopCalibrating();
+ }));
+ }
}
@Override
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
public class TrenchAssistConstants {
- public static final Rectangle2d[] OBSTACLES = new Rectangle2d[]{
- new Rectangle2d(new Translation2d(4.03, 1.28), new Translation2d(5.22, 1.58)),
- new Rectangle2d(new Translation2d(4.03, 8.07 - 1.28), new Translation2d(5.22, 8.07 - 1.58)),
- new Rectangle2d(new Translation2d(11.32, 1.28), new Translation2d(12.51, 1.58)),
- new Rectangle2d(new Translation2d(11.32, 8.07 - 1.28), new Translation2d(12.51, 8.07 - 1.58)),
- }; //8.07m
+ public static final Rectangle2d[] OBSTACLES = new Rectangle2d[] {
+ new Rectangle2d(new Translation2d(4.03, 1.28), new Translation2d(5.22, 1.58)),
+ new Rectangle2d(new Translation2d(4.03, 8.07 - 1.28), new Translation2d(5.22, 8.07 - 1.58)),
+ new Rectangle2d(new Translation2d(11.32, 1.28), new Translation2d(12.51, 1.58)),
+ new Rectangle2d(new Translation2d(11.32, 8.07 - 1.28), new Translation2d(12.51, 8.07 - 1.58)),
+ }; // 8.07m
- public static final Rectangle2d[] ALIGN_ZONES = new Rectangle2d[]{
- new Rectangle2d(new Translation2d(4.03 - .5, 0), new Translation2d(5.22 + .5, 3)),
- new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)),
- new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)),
- new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)),
- new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)),
+ public static final Rectangle2d[] ALIGN_ZONES = new Rectangle2d[] {
+ new Rectangle2d(new Translation2d(4.03 - .5, 0), new Translation2d(5.22 + .5, 3)),
+ new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)),
+ new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)),
+ new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)),
+ new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)),
};
- public static final double[] SLIDE_LATITUDES = new double[] {
- // 8.07 - Units.inchesToMeters(30.0),
- // Units.inchesToMeters(30.0), should be accurate, i think our field is slightly
- // too small
- 6.550,
- 0.668,
+ public static final double[] SLIDE_LATITUDES = new double[]{
+ 8.07 - Units.inchesToMeters(30.0),
+ Units.inchesToMeters(30.0), // should be accurate, i think our field is slightly too small
+ // 6.550,
+ // 0.668,
};