Merge branch 'main' into Boolean-condition-for-auto

Team488 · Mar 30, 2024 · 8350e3e · 8350e3e
2 parents 4787173 + 01d9a66
commit 8350e3e
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 15 deletions.
diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java
@@ -59,7 +59,6 @@ protected void initializeSystems() {
         var autoSelector = getInjectorComponent().autonomousCommandSelector();
 
         autoSelector.setCurrentAutonomousCommand(defaultAuto);
-        autoSelector.setIsDefault(true);
         LogTable.disableProtobufWarning();
     }
 

diff --git a/src/main/java/competition/subsystems/lights/LightSubsystem.java b/src/main/java/competition/subsystems/lights/LightSubsystem.java
@@ -9,12 +9,15 @@
 import competition.subsystems.collector.CollectorSubsystem;
 import competition.subsystems.oracle.DynamicOracle;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
+import competition.subsystems.vision.VisionSubsystem;
 import edu.wpi.first.wpilibj.DriverStation;
 import xbot.common.command.BaseSubsystem;
 import xbot.common.controls.actuators.XDigitalOutput;
 import xbot.common.controls.actuators.XDigitalOutput.XDigitalOutputFactory;
 import xbot.common.subsystems.autonomous.AutonomousCommandSelector;
 
+import java.util.Objects;
+
 @Singleton
 public class LightSubsystem extends BaseSubsystem {
     // based on the number of bits we have, this is the highest number we can send
@@ -24,6 +27,7 @@ public class LightSubsystem extends BaseSubsystem {
     final AutonomousCommandSelector autonomousCommandSelector;
     final ShooterWheelSubsystem shooter;
     final CollectorSubsystem collector;
+    final VisionSubsystem vision;
     final DynamicOracle oracle;
     final XDigitalOutput[] outputs;
 
@@ -32,8 +36,12 @@ public class LightSubsystem extends BaseSubsystem {
     public enum LightsStateMessage{
         NoCode(15), // we never send this one, it's implicit when the robot is off
         // and all of the DIOs float high
-        WithDefaultAuto(7),
-        WithCustomAuto(6),
+        DisabledCustomAutoSomeCamerasWorking(13),
+        DisabledCustomAutoNoCamerasWorking(12),
+        DisabledCustomAutoAllCamerasWorking(11),
+        DisabledDefaultAutoSomeCamerasWorking(10),
+        DisabledDefaultAutoNoCameraWorking(9),
+        DisabledDefaultAutoAllCamerasWorking(8),
         RobotEnabled(5),
         ShooterReadyWithoutNote(1),
         ReadyToShoot(2),
@@ -57,17 +65,28 @@ public int getValue() {
             }
             return value;
         }
+
+        public static LightsStateMessage getStringValueFromInt(int i) {
+            for (LightsStateMessage states : LightsStateMessage.values()) {
+                if (states.getValue() == i) {
+                    return states;
+                }
+            }
+           return LightsStateMessage.NoCode;
+        }
     }
 
     @Inject
     public LightSubsystem(XDigitalOutputFactory digitalOutputFactory,
                           ElectricalContract contract,
                           AutonomousCommandSelector autonomousCommandSelector,
                           ShooterWheelSubsystem shooter, CollectorSubsystem collector,
+                          VisionSubsystem vision,
                           DynamicOracle oracle) {
         this.autonomousCommandSelector = autonomousCommandSelector;
         this.collector = collector;
         this.shooter = shooter;
+        this.vision = vision;
         this.oracle = oracle;
         this.outputs = new XDigitalOutput[numBits];
         this.outputs[0] = digitalOutputFactory.create(contract.getLightsDio0().channel);
@@ -85,19 +104,14 @@ public LightsStateMessage getCurrentState() {
         // Not sure about if the way we are checking the shooter is correct (and collector)
         if (!dsEnabled) {
             // Check if auto program is set
-            //isDefault = pf.createPersistentProperty("IsDefaultAuto", autonomousCommandSelector.getIsDefault()?1.0:2.0);
-            if (autonomousCommandSelector.getIsDefault()) {
-                currentState = LightsStateMessage.WithDefaultAuto;
-            } else {
-                currentState = LightsStateMessage.WithCustomAuto;
+            int base = 8;
+            if (!Objects.equals(autonomousCommandSelector.getProgramName(), "SubwooferShotFromMidShootThenShootNearestThree")) {
+                // Not default
+                base = 11;
             }
-
+            // 0 as no camera working, 1 as all camera working, 2 as some camera working
+            currentState = LightsStateMessage.getStringValueFromInt(base + vision.cameraWorkingState());
         } else {
-            // Try and match enabled states
-            //if (ampSignalOn) {
-                //currentState = LightsStateMessage.AmpSignal;
-
-            //} else
             if (shooter.isReadyToFire() && collector.checkSensorForLights()) {
                 currentState = LightsStateMessage.ReadyToShoot;
 
@@ -114,7 +128,6 @@ public LightsStateMessage getCurrentState() {
                 currentState = LightsStateMessage.RobotEnabled;
             }
         }
-
         return currentState;
     }
 

diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java
@@ -462,4 +462,17 @@ public void refreshDataFrame() {
         }
         centerlineNoteCamera.refreshDataFrame();
     }
+
+    public int cameraWorkingState() {
+        if (allCameras.stream().allMatch(state -> state.isCameraWorking())) {
+            // If all are working, return 0
+            return 0;
+        }
+        else if (allCameras.stream().allMatch(state -> !state.isCameraWorking())) {
+            // If no cameras are working, return 1
+            return 1;
+        }
+        // If some of the cameras are working, return 2
+        return 2;
+    }
 }