did some crazy led code, hoping it works

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

@@ -173,7 +173,7 @@ public static class Presets {
   public static class Ports {
     public static final int DRIVER_CONTROLLER = 0;
     public static final int OPERATOR_CONTROLLER = 1;
-    public static final int LEDS = 0; // pwm port
+    public static final int LEDS = 1; // pwm port
   }
 
   // static field constants

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

@@ -52,7 +52,7 @@ public class RobotContainer {
   private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem);
   private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem();
   private final IntakeSubsystem _intakeSubsystem = new IntakeSubsystem();
-  private final LEDSubsystem _ledSubsystem = new LEDSubsystem(0, 100);
+  private final LEDSubsystem _ledSubsystem = new LEDSubsystem(Constants.Ports.LEDS, 35);
   private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem();
 
   // controllers (for driver and operator)
@@ -111,7 +111,10 @@ public RobotContainer() {
 
     // Non drive/operate default commands
     _intakeSubsystem.setDefaultCommand(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.NONE));
-    _ledSubsystem.setDefaultCommand(new DefaultLED(_ledSubsystem));
+    _ledSubsystem.setDefaultCommand(new DefaultLED(
+      _ledSubsystem,
+      () -> _swerveSubsystem.atDesiredHeading() && _elevatorSubsystem.atDesiredHeight() && _shooterSubsystem.atDesiredAngle()
+    ));
 
     configureBindings();
 

src/main/java/frc/robot/commands/auto/AutoAim.java

@@ -6,19 +6,25 @@
 
 import java.util.function.DoubleSupplier;
 
+import org.ejml.dense.row.decomposition.UtilDecompositons_FDRM;
+
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.elevator.SetElevator;
 import frc.robot.commands.shooter.SetShooter;
 import frc.robot.commands.swerve.SetHeading;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
+import frc.robot.subsystems.LEDSubsystem.LEDState;
+import frc.robot.utils.UtilFuncs;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class AutoAim extends ParallelCommandGroup {
+public class AutoAim extends SequentialCommandGroup {
   /** 
    * Creates a new AutoAim.
    * 
@@ -53,10 +59,11 @@ private AutoAim(
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
-      new SetShooter(shooter, shooterAngle, runOnce),
-      new SetElevator(elevator, elevatorHeight, runOnce)
-      // some led command here
+      new ParallelCommandGroup(
+        new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
+        new SetShooter(shooter, shooterAngle, runOnce),
+        new SetElevator(elevator, elevatorHeight, runOnce)
+      ).beforeStarting(() -> UtilFuncs.SetLEDs(LEDState.AIM)).finallyDo(() -> UtilFuncs.SetLEDs(LEDState.DEFAULT))
     );
   }
 

src/main/java/frc/robot/commands/leds/DefaultLED.java

@@ -1,22 +1,33 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.commands.leds;
 
+import java.util.function.BooleanSupplier;
+
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.Constants.LEDColors;
 import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.SwerveDriveSubsystem;
 import frc.robot.utils.UtilFuncs;
 
 /**
  * @author Lucas Ou
  */
 public class DefaultLED extends Command {
-  private LEDSubsystem _leds;
+  private final LEDSubsystem _leds;
+
+  private final BooleanSupplier _isAimed;
 
   /** Creates a new TestLED. */
-  public DefaultLED(LEDSubsystem leds) {
+  public DefaultLED(
+    LEDSubsystem leds,
+    BooleanSupplier isAimed
+  ) {
     // Use addRequirements() here to declare subsystem dependencies.
     _leds = leds;
+    _isAimed = isAimed;
+
     addRequirements(_leds);
   }
 
@@ -28,7 +39,22 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    _leds.setColor(UtilFuncs.GetAlliance() == Alliance.Red ? LEDColors.RED : LEDColors.BLUE);
+    switch (UtilFuncs.GetLEDs()) {
+      case DEFAULT:
+        _leds.setColor(UtilFuncs.GetAlliance() == Alliance.Red ? LEDColors.RED : LEDColors.BLUE);
+        break;
+
+      case AIM:
+        if (!_isAimed.getAsBoolean()) {
+          _leds.blink(Constants.LEDColors.YELLOW, Constants.LEDColors.NOTHING, 1);
+        } else {
+          _leds.setColor(Constants.LEDColors.GREEN);
+        }
+        break;
+
+      default:
+        break;
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/LEDSubsystem.java

@@ -23,6 +23,11 @@ public class LEDSubsystem extends SubsystemBase {
 
   private Timer _ledTimer = new Timer();
 
+  public enum LEDState {
+    DEFAULT,
+    AIM
+  }
+
   /** Creates a new LEDSubsystem. */
   public LEDSubsystem(int port, int ledNumber) {
     _ledNumber = ledNumber;

src/main/java/frc/robot/utils/UtilFuncs.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.Constants.FieldConstants;
+import frc.robot.subsystems.LEDSubsystem.LEDState;
 import frc.robot.utils.helpers.AllianceHelper;
 
 /** Any utility functions are here. */
@@ -23,6 +24,16 @@ public final class UtilFuncs {
   private static Pose3d SPEAKER_RED_POSE;
   private static Pose3d SPEAKER_BLUE_POSE;
 
+  private static LEDState _ledState = LEDState.DEFAULT;
+
+  public static void SetLEDs(LEDState ledState) {
+    _ledState = ledState;
+  }
+
+  public static LEDState GetLEDs() {
+    return _ledState;
+  }
+
   /**
    * Converts limelight pose data into wpilib "Pose2d".
    *