From 25ff1602085a2e58cd0e3ef9da4b40ce124c6807 Mon Sep 17 00:00:00 2001
From: Griffin <lamb.griffin@icloud.com>
Date: Tue, 12 Mar 2024 20:07:30 -0400
Subject: [PATCH] worked on java side of LED Code

---
 .../java/frc/robot/subsystems/SwagLights.java | 46 +++++++++++++++----
 1 file changed, 36 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwagLights.java b/src/main/java/frc/robot/subsystems/SwagLights.java
index 96067383..69b18708 100644
--- a/src/main/java/frc/robot/subsystems/SwagLights.java
+++ b/src/main/java/frc/robot/subsystems/SwagLights.java
@@ -4,30 +4,44 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
 public class SwagLights implements Subsystem {
+
+    private Acquisition acquisition = Acquisition.getInstance();
+    private Storage storage = Storage.getInstance();
+
+    private boolean noteSeenAcquire = false;
+    private boolean noteSeenStorage = false;
+
     // Enums
     public enum RobotStates {
         Enabled("A"),
         Disabled("D"),
         EmergencyStop("E");
 
-        public final String value;
+        private final String value;
 
         private RobotStates(String value) {
             this.value = value;
         }
+
+        public String getRobotValue() {
+            return this.value;
+        }
     }
 
     public enum OperatorStates {
-        Cone("Y"),
-        Cube("P"),
-        Hybrid("H"),
-        AcquireSuccess("G");
+        Default("X"),
+        NoteSeen("N"),
+        NoteAcquired("G");
 
-        public final String value;
+        private final String value;
 
         private OperatorStates(String value) {
             this.value = value;
         }
+
+        public String getOperatorValue() {
+            return this.value;
+        }
     }
 
     // Inputs and Outputs
@@ -35,7 +49,7 @@ private OperatorStates(String value) {
 
     // States
     private RobotStates robotState = RobotStates.Disabled;
-    private OperatorStates operatorState = OperatorStates.Cube;
+    private OperatorStates operatorState = OperatorStates.Default;
 
     // Singleton Setup
     private static SwagLights instance;
@@ -59,11 +73,23 @@ private SwagLights() {
     @Override
     public void periodic() {
         if (this.robotState == RobotStates.Enabled) {
+            if (acquisition.isNotePresent()) {
+                this.operatorState = OperatorStates.NoteAcquired;
+                noteSeenAcquire = true;
+            }
+            if (noteSeenAcquire && !noteSeenStorage && storage.noteExitingStorage()) {
+                noteSeenStorage = true;
+                noteSeenAcquire = false;
+            }
+            if (!noteSeenAcquire && noteSeenStorage && !storage.noteExitingStorage()) {
+                noteSeenStorage = false;
+                this.operatorState = OperatorStates.Default;
+            }
             setLedStates(
-                    this.robotState.value,
-                    this.operatorState.value);
+                    this.robotState.getRobotValue(),
+                    this.operatorState.getOperatorValue());
         } else {
-            setLedStates(this.robotState.value);
+            setLedStates(this.robotState.getRobotValue());
         }
     }