worked on java side of LED Code

LakotaRobotics1038 · Mar 13, 2024 · 25ff160 · 25ff160
1 parent 3b6cea7
commit 25ff160
Showing 1 changed file with 36 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/subsystems/SwagLights.java b/src/main/java/frc/robot/subsystems/SwagLights.java
@@ -4,38 +4,52 @@
 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
     private SerialPort serialPort;
 
     // 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());
         }
     }