From: iefomit <timofei.stem@gmail.com>
Date: Wed, 15 Apr 2026 22:14:03 +0000 (-0700)
Subject: Merge branch 'main' into taran-cleanout
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=05ba394a5a5114c6c60bcf582b14be56ae94c999;p=FRC2026.git

Merge branch 'main' into taran-cleanout
---

05ba394a5a5114c6c60bcf582b14be56ae94c999
diff --cc src/main/java/frc/robot/commands/gpm/Superstructure.java
index 9954633,4853269..f5a11f5
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@@ -241,12 -238,16 +241,6 @@@ public class Superstructure extends Com
  
      @Override
      public void execute() {
-         // If we ever need to lose a match use this code below
-         // SmartDashboard.putData("Hood: Shoot Higher", new InstantCommand(() -> bumpUpHoodOffset()));
-         // SmartDashboard.putData("Hood: Shoot Lower", new InstantCommand(() -> bumpDownHoodOffset()));
-         // SmartDashboard.putData("Turret: Aim Left", new InstantCommand(() -> bumpUpTurretOffset()));
-         // SmartDashboard.putData("Turret: Aim Right", new InstantCommand(() -> bumpDownTurretOffset()));
 -        if (!Constants.DISABLE_SMART_DASHBOARD) {
 -            TOFAdjustment = SmartDashboard.getNumber("OPERATOR: TOF Adjustment", TOFAdjustment);
 -            SmartDashboard.putNumber("OPERATOR: TOF Adjustment", TOFAdjustment);
 -            hoodOffset = SmartDashboard.getNumber("OPERATOR: Hood Offset", hoodOffset);
 -            SmartDashboard.putNumber("OPERATOR: Hood Offset", hoodOffset);
 -        }
 -
 -        turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset);
 -        SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
--
          // Phase manager stuff
          phaseManager.update(drivepose, shooter, turret);
          target = phaseManager.getTarget(drivepose);
diff --cc src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index c6e2ce6,9b57820..b2a9341
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@@ -54,151 -56,154 +54,150 @@@ public class PS5ControllerDriverConfig 
          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;
 +  }
  }