From f48e6ed8ce943c2d3bbc405ab7a29e62f48995be Mon Sep 17 00:00:00 2001
From: Lev Strougov <62769580+Levercpu@users.noreply.github.com>
Date: Sat, 22 Mar 2025 23:03:39 -0400
Subject: [PATCH] added colors

---
 .../commands/alignment/AlignClosestBranch.java | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java
index 2d21e13e..1df01f8f 100644
--- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java
+++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java
@@ -4,7 +4,9 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.constants.AlignmentPosition;
 import frc.robot.constants.Constants;
+import frc.robot.subsystems.lightStrip.LightStrip;
 import frc.robot.subsystems.swervev3.SwerveDrivetrain;
+import frc.robot.utils.BlinkinPattern;
 import frc.robot.utils.logging.commands.LoggableCommand;
 import frc.robot.utils.logging.commands.LoggableCommandWrapper;
 import org.littletonrobotics.junction.Logger;
@@ -12,12 +14,14 @@
 public class AlignClosestBranch extends LoggableCommand {
   private AlignmentPosition alignmentTarget;
   private final SwerveDrivetrain drivetrain;
+  private final LightStrip light;
   private LoggableCommand followTrajectory;
   private final Timer timer = new Timer();
 
-  public AlignClosestBranch(SwerveDrivetrain drivetrain) {
+  public AlignClosestBranch(SwerveDrivetrain drivetrain, LightStrip lightStrip) {
     this.drivetrain = drivetrain;
-    addRequirements(drivetrain);
+    this.light = lightStrip;
+    addRequirements(drivetrain, lightStrip);
   }
 
   @Override
@@ -46,6 +50,14 @@ public void end(boolean interrupted) {
 
   @Override
   public boolean isFinished() {
-    return followTrajectory.isFinished() || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT);
+    if (followTrajectory.isFinished()) {
+      light.setPattern(BlinkinPattern.LAWN_GREEN);
+      return true;
+    } else if (timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT)) {
+      light.setPattern(BlinkinPattern.HEARTBEAT_RED);
+      return true;
+    } else {
+      return false;
+    }
   }
 }