Cleanup

src/main/java/frc/robot/Constants.java

@@ -33,8 +33,6 @@ public final class Constants {
   public static final class FeatureFlags {
     // subsystems
 
-    public static final boolean kEasterEggEnabled = false;
-
     public static final boolean kIntakeEnabled = true;
 
     public static final boolean kShooterEnabled = true;
@@ -45,7 +43,6 @@ public static final class FeatureFlags {
     public static final boolean kPivotIntakeEnabled = true;
 
     public static final boolean kClimbEnabled = true;
-    public static final boolean kLEDEnabled = true;
 
     public static final boolean kAmpBarEnabled = true;
 
@@ -55,24 +52,6 @@ public static final class FeatureFlags {
     public static final boolean DebugCommandEnabled = false;
     public static final boolean kRobotVizEnabled = true && !Robot.isReal();
 
-    // features
-    public static final boolean kAutoAlignEnabled = false;
-    public static final boolean kIntakeAutoScoreDistanceSensorOffset = false;
-    public static final boolean kShuffleboardLayoutEnabled = true;
-    public static final boolean kUsePrefs = true;
-
-    public static final boolean kPitRoutineEnabled = false;
-
-    public static final boolean kCanTestEnabled = false;
     public static boolean kPivotShooterEnabled = true;
   }
-
-  public static final class ShuffleboardConstants {
-    public static final String kDriverTabName = "Driver";
-    public static final String kOperatorTabName = "Operator";
-    public static final String kIntakeLayoutName = "Intake";
-    public static final String kSwerveLayoutName = "Swerve";
-    public static final String kArmLayoutName = "Arm";
-    public static final String kShooterLayoutName = "Shooter";
-  }
 }

src/main/java/frc/robot/Robot.java

@@ -252,7 +252,6 @@ public void testInit() {
     CommandScheduler.getInstance().cancelAll();
     // Run CAN Test
     // m_robotContainer.CANTest();
-    m_robotContainer.runPitTestRoutine();
   }
 
   /** This function is called periodically during test mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -11,7 +11,6 @@
 import static frc.robot.subsystems.pivotshooter.PivotShooterConstants.*;
 import static frc.robot.subsystems.swerve.AzimuthConstants.*;
 
-import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
@@ -29,6 +28,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.FeatureFlags;
 import frc.robot.autos.commands.IntakeSequence;
+import frc.robot.autos.routines.AutoRoutines;
 import frc.robot.helpers.XboxStalker;
 import frc.robot.subsystems.ampbar.AmpBar;
 import frc.robot.subsystems.ampbar.AmpBarIOTalonFX;
@@ -38,7 +38,6 @@
 import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.IntakeIOTalonFX;
 import frc.robot.subsystems.led.LED;
-import frc.robot.subsystems.led.commands.*;
 import frc.robot.subsystems.pivotintake.PivotIntake;
 import frc.robot.subsystems.pivotintake.PivotIntakeConstants;
 import frc.robot.subsystems.pivotintake.PivotIntakeIOTalonFX;
@@ -55,7 +54,6 @@
 import frc.robot.subsystems.swerve.requests.SwerveFieldCentricFacingAngle;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIOLimelight;
-import frc.robot.utils.CommandQueue;
 import org.littletonrobotics.junction.Logger;
 
 /**
@@ -69,7 +67,6 @@ public class RobotContainer {
   /* Controllers */
   private final CommandXboxController driver = new CommandXboxController(0);
   private final CommandXboxController operator = new CommandXboxController(1);
-  private final CommandXboxController test = new CommandXboxController(2);
 
   /* Drive Controls */
   private final int translationAxis = XboxController.Axis.kLeftY.value;
@@ -107,7 +104,6 @@ public class RobotContainer {
   public AmpBar ampbar;
   public PivotIntake pivotIntake;
   public Climb climb;
-  public CommandQueue commandQueue;
 
   public Vision vision;
 
@@ -122,7 +118,6 @@ public RobotContainer() {
     // Cancel any previous commands running
     CommandScheduler.getInstance().cancelAll();
 
-    commandQueue = new CommandQueue();
     vision = new Vision(new VisionIOLimelight());
 
     // Setup subsystems & button-bindings
@@ -132,17 +127,6 @@ public RobotContainer() {
     configurePivotIntake();
     configureIntake();
     configureSwerve();
-    test.leftBumper().onTrue(Commands.runOnce(SignalLogger::start));
-    test.rightBumper().onTrue(Commands.runOnce(SignalLogger::stop));
-
-    test.leftTrigger()
-        .onTrue(
-            drivetrain.applyRequest(() -> azi.withTargetDirection(Rotation2d.fromDegrees(180))));
-
-    // test.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    // test.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    // test.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    // test.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
     configureClimb();
     // If all the subsystems are enabled, configure "operator" autos
@@ -152,10 +136,6 @@ public RobotContainer() {
       configureOperatorAutos();
     }
 
-    if (FeatureFlags.kLEDEnabled) {
-      configureLED();
-    }
-
     // Named commands
     {
       // Auto named commands
@@ -298,31 +278,19 @@ public RobotContainer() {
                   pivotIntake.setPosition(kPivotGroundPos).withTimeout(0.75),
                   pivotIntake.slamAndPID())));
     }
-
     /* Run checks */
     configureCheeks();
 
-    // operator.povLeft().onTrue(cancelCommand);
-    // Configure the auto
-    //    if (FeatureFlags.kSwerveEnabled) {
-    //      autoChooser = AutoBuilder.buildAutoChooser();
-    //    } else {
-    //      autoChooser = new SendableChooser<>();
-    //    }
     autoChooser = new SendableChooser<>();
     autoChooser.setDefaultOption("Do Nothing", new InstantCommand());
-    // autoChooser.addOption(
-    //     "5 Note test",
-    //     AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake));
-    // Autos
+    autoChooser.addOption(
+        "5 Note test",
+        AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake));
     SmartDashboard.putData("Auto Chooser", autoChooser);
   }
 
   private void configurePivotShooter() {
     pivotShooter = new PivotShooter(FeatureFlags.kPivotShooterEnabled, new PivotShooterIOTalonFX());
-    // operator.b().onTrue(new bruh(pivotShooter));
-    // operator.x().onTrue(new SequentialCommandGroup(new
-    // PivotShootSubwoofer(pivotShooter)));
     operator
         .x()
         .onTrue(
@@ -380,11 +348,8 @@ private void configurePivotIntake() {
 
   private void configureClimb() {
     climb = new Climb(FeatureFlags.kClimbEnabled, new ClimbIOTalonFX());
-    // zeroClimb = new ZeroClimb(climb); // NEED FOR SHUFFLEBOARD
-
     operator.povDown().onTrue(climb.zero());
-    // new Trigger(() -> operator.getRawAxis(translationAxis) < -0.5).onTrue(new
-    // UpClimb(climb));
+
     new Trigger(() -> operator.getRawAxis(translationAxis) > 0.5).onTrue(climb.retractClimber());
     if (this.ampbar != null && this.pivotShooter != null) {
       new Trigger(() -> operator.getRawAxis(translationAxis) < -0.5)
@@ -398,11 +363,6 @@ private void configureClimb() {
       new Trigger(() -> Math.abs(operator.getRawAxis(secondaryAxis)) > 0.5)
           .onTrue(new PrintCommand("u suck")); // old command waws dehook climb
     }
-    // Josh: HangSequence is broken and Rhea does not want to use it; we should
-    // rmove this
-    // later.
-    // new ScheduleCommand(new HangSequence(climb, operator)).schedule();
-    // operator.povDownLeft().onTrue(new TestClimbFlip(climb));
   }
 
   public void setAllianceCol(boolean col) {
@@ -494,17 +454,6 @@ public void configureSwerve() {
 
   private void configureShooter() {
     shooter = new Shooter(FeatureFlags.kShooterEnabled, new ShooterIOTalonFX());
-    // new Trigger(() -> Math.abs(shooter.getShooterRps() - 100) <= 5)
-    // .onTrue(
-    // new InstantCommand(
-    // () -> {
-    // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 100);
-    // }))
-    // .onFalse(
-    // new InstantCommand(
-    // () -> {
-    // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0);
-    // }));
     if (FeatureFlags.kAmpBarEnabled) {
       ampbar = new AmpBar(FeatureFlags.kAmpBarEnabled, new AmpBarIOTalonFX());
       operator
@@ -565,82 +514,6 @@ public void disableRumble() {
     operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0);
   }
 
-  public void configureLED() {
-    int[][] ledList = new int[][] {new int[] {2, 3}, new int[] {1, 1}};
-
-    led = new LED();
-    led.setDefaultCommand(new CoordinatesButItsMultiple(led, ledList, 100, 0, 0, 10));
-    // led.setDefaultCommand(new SetLEDsFromBinaryString(led, LEDConstants.based,
-    // 100, 0, 0, 5));
-
-    /*
-     * Intake LED, flashes RED while intake is down and running,
-     * flashes GREEN on successful intake
-     */
-    if (FeatureFlags.kIntakeEnabled) {
-      // Trigger intakeDetectedNote = new Trigger(intake::isBeamBroken);
-      // // intakeDetectedNote.whileTrue(new SetSuccessfulIntake(led));
-
-      // intakeDetectedNote
-      // .onTrue(
-      // new InstantCommand(
-      // () -> {
-      // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 50);
-      // driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 50);
-      // }))
-      // .onFalse(
-      // new InstantCommand(
-      // () -> {
-      // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0);
-      // driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0);
-      // }));
-
-      // This boolean is true when velocity is LESS than 0.
-      // Trigger intakeRunning = new Trigger(intake::isMotorSpinning);
-      // intakeRunning.whileTrue(new SetGroundIntakeRunning(led));
-    }
-
-    /*
-     * Shooter LED, solid ORANGE while shooter is running, flashes ORANGE if note
-     * fed
-     */
-    // if (FeatureFlags.kShooterEnabled) {
-
-    // Trigger shooterRunning = new Trigger(
-    // () -> (shooter.getShooterFollowerRps() > 75 || shooter.getShooterRps() >
-    // 75));
-    // shooterRunning.whileTrue(new SetSpeakerScore(led));
-    // }
-    // if (FeatureFlags.kSwerveEnabled) {
-    // if (DriverStation.isAutonomousEnabled() ||
-
-    // FeatureFlags.kSwerveUseVisionForPoseEst) {
-    // Trigger swerveSpeakerAligned = new Trigger(swerveDrive::isAlignedToSpeaker);
-    // swerveSpeakerAligned.whileTrue(new SetRobotAligned(led));
-    // }
-    // if (DriverStation.isTeleopEnabled()) {
-    // // if (DriverStation.isTeleopEnabled()) {
-    // // Trigger azimuthRan =
-    // // new Trigger(
-    // // () ->
-    // // (driver.leftBumper().getAsBoolean() ||
-    // driver.rightBumper().getAsBoolean())
-    // // || driver.x().getAsBoolean()
-    // // || driver.b().getAsBoolean()
-    // // || driver.a().getAsBoolean()
-    // // || driver.povUp().getAsBoolean());
-    // // azimuthRan.whileTrue(new SetAzimuthRan(led));
-    // // }
-    // }
-
-    // if (FeatureFlags.kClimbEnabled) {
-    // Trigger climbRunning =
-    // new Trigger(
-    // () -> ((climb.getLeftCurrent() > 0) || (operator.povDown().getAsBoolean())));
-    // }
-    // }
-  }
-
   private void configureCheeks() {
     // Here, put checks for the configuration of the robot
     if (DriverStation.isFMSAttached()) {
@@ -673,14 +546,6 @@ public Command getAutonomousCommand() {
     return autoChooser.getSelected();
   }
 
-  /* Test Routines */
-
-  public void runPitTestRoutine() {
-    // Command pitRoutine = new PitRoutine(swerveDrive, climb, intake, pivotIntake,
-    // shooter);
-    // pitRoutine.schedule();
-  }
-
   public void periodic(double dt) {
     XboxStalker.stalk(driver, operator);
     Logger.recordOutput("Note pose", vision.getNotePose(drivetrain.getState().Pose));

src/main/java/frc/robot/autos/AutoConstants.java

@@ -7,8 +7,6 @@
 
 package frc.robot.autos;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public final class AutoConstants {
@@ -17,22 +15,6 @@ public final class AutoConstants {
   public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
   public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-  public static final Pose2d kRedSpeakerLocation =
-      new Pose2d(15.26, 5.56, Rotation2d.fromDegrees(-180));
-
-  public static final Pose2d kBlueSpeakerLocation =
-      new Pose2d(1.27, 5.56, Rotation2d.fromDegrees(180));
-
-  public static final Pose2d kRedAmpLocation = new Pose2d(14.74, 7.48, Rotation2d.fromDegrees(90));
-
-  public static final Pose2d kBlueAmpLocation = new Pose2d(1.81, 7.57, Rotation2d.fromDegrees(90));
-
-  public static final double kSpeakerAlignmentThreshold = 0.2; // meters
-
-  public static final double kPXController = 1;
-  public static final double kPYController = 1;
-  public static final double kPThetaController = 1;
-
   /* Constraint for the motion profilied robot angle controller */
   public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
       new TrapezoidProfile.Constraints(