LED_Limelite

Robocubs · Jun 24, 2024 · a212864 · a212864
1 parent ff497fe
commit a212864
Show file tree

Hide file tree

Showing 8 changed files with 125 additions and 13 deletions.
diff --git a/src/main/java/com/team1701/lib/drivers/cameras/neural/DetectorCamera.java b/src/main/java/com/team1701/lib/drivers/cameras/neural/DetectorCamera.java
@@ -104,4 +104,8 @@ public void addDetectedObjectConsumer(Consumer<List<DetectedObjectState>> consum
     public void addPoseFilter(Predicate<Pose3d> filter) {
         mPoseFilters.add(filter);
     }
+
+    public boolean isConnected() {
+        return mDetectorCameraInputs.isConnected;
+    }
 }
diff --git a/src/main/java/com/team1701/lib/drivers/leds/LEDController.java b/src/main/java/com/team1701/lib/drivers/leds/LEDController.java
@@ -1,6 +1,7 @@
 package com.team1701.lib.drivers.leds;
 
 import java.util.Arrays;
+import java.util.Optional;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.AddressableLED;
@@ -54,6 +55,87 @@ public void setRange(int start, int end, Color color, double brightness) {
         Arrays.fill(mBrightness, start, end, toBrightnessValue(brightness));
     }
 
+    public void setRange(
+            int start,
+            int firstStop,
+            int secondStart,
+            int secondStop,
+            int thirdStart,
+            int thirdStop,
+            int fourthStart,
+            int end,
+            Color color,
+            Color state) {
+        Arrays.fill(mColors, start, firstStop, color);
+        Arrays.fill(mColors, firstStop, secondStart, state);
+        Arrays.fill(mColors, secondStart, secondStop, color);
+        Arrays.fill(mColors, secondStop, thirdStart, state);
+        Arrays.fill(mColors, thirdStart, thirdStop, color);
+        Arrays.fill(mColors, thirdStop, fourthStart, state);
+        Arrays.fill(mColors, fourthStart, end, color);
+        Arrays.fill(mBrightness, start, end, kMaxBrightness);
+    }
+
+    public void setRange(
+            int start,
+            int firstStop,
+            int secondStart,
+            int secondStop,
+            int thirdStart,
+            int thirdStop,
+            int fourthStart,
+            int end,
+            Color color,
+            Color state,
+            double brightness) {
+        Arrays.fill(mColors, start, firstStop, color);
+        Arrays.fill(mColors, firstStop, secondStart, state);
+        Arrays.fill(mColors, secondStart, secondStop, color);
+        Arrays.fill(mColors, secondStop, thirdStart, state);
+        Arrays.fill(mColors, thirdStart, thirdStop, color);
+        Arrays.fill(mColors, thirdStop, fourthStart, state);
+        Arrays.fill(mColors, fourthStart, end, color);
+        Arrays.fill(mBrightness, start, end, toBrightnessValue(brightness));
+    }
+
+    public void setAll(Color state, Optional<Boolean> detectorCamConnected) {
+        if (detectorCamConnected.get() != false) {
+            setAll(state);
+        } else {
+            setRange(0, 2, 23, 28, 49, 54, 75, 77, Color.kYellow, state);
+        }
+    }
+
+    public void setAll(Color state, double brightness, Optional<Boolean> detectorCamConnected) {
+        if (detectorCamConnected.get() != false) {
+            setAll(state, brightness);
+        } else {
+            setRange(0, 2, 23, 28, 49, 54, 75, 77, Color.kYellow, state, brightness);
+        }
+    }
+
+    public void set(int index, Color color, Optional<Boolean> detectorCamConnected) {
+        if (detectorCamConnected.get() != false) {
+            mColors[index] = color;
+            mBrightness[index] = kMaxBrightness;
+        } else {
+            setRange(0, 2, 23, 28, 49, 54, 75, 77, Color.kYellow, color, kMaxBrightness);
+            mColors[index] = color;
+            mBrightness[index] = kMaxBrightness;
+        }
+    }
+
+    public void set(int index, Color color, double brightness, Optional<Boolean> detectorCamConnected) {
+        if (detectorCamConnected.get() != false) {
+            mColors[index] = color;
+            mBrightness[index] = toBrightnessValue(brightness);
+        } else {
+            setRange(0, 2, 23, 28, 49, 54, 75, 77, Color.kYellow, color, toBrightnessValue(brightness));
+            mColors[index] = color;
+            mBrightness[index] = toBrightnessValue(brightness);
+        }
+    }
+
     private int toBrightnessValue(double brightness) {
         return MathUtil.clamp((int) (brightness * kMaxBrightness), 0, kMaxBrightness);
     }

diff --git a/src/main/java/com/team1701/lib/drivers/leds/mRobotState.java b/src/main/java/com/team1701/lib/drivers/leds/mRobotState.java
@@ -0,0 +1,3 @@
+package com.team1701.lib.drivers.leds;
+
+public enum mRobotState {}
diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java
@@ -299,7 +299,7 @@ public RobotContainer() {
 
         mLED = new LED(mRobotState);
 
-        mRobotState.addSubsystems(this.mDrive, this.mShooter, this.mIndexer, this.mIntake);
+        mRobotState.addSubsystems(this.mDrive, this.mShooter, this.mIndexer, this.mIntake, this.mVision);
 
         SmartDashboard.putData(mDrive);
         SmartDashboard.putData(mShooter);

diff --git a/src/main/java/com/team1701/robot/states/RobotState.java b/src/main/java/com/team1701/robot/states/RobotState.java
@@ -23,6 +23,7 @@
 import com.team1701.robot.subsystems.indexer.Indexer;
 import com.team1701.robot.subsystems.intake.Intake;
 import com.team1701.robot.subsystems.shooter.Shooter;
+import com.team1701.robot.subsystems.vision.Vision;
 import com.team1701.robot.util.FieldUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -58,6 +59,7 @@ public class RobotState {
     private Optional<Indexer> mIndexer = Optional.empty();
     private Optional<Intake> mIntake = Optional.empty();
     private Optional<Shooter> mShooter = Optional.empty();
+    private Optional<Vision> mVision = Optional.empty();
 
     private ShootingState mShootingState = ShootingState.kDefault;
 
@@ -86,11 +88,12 @@ public RobotState() {
     private List<DetectedObjectState> mDetectedNotes = new ArrayList<>();
     private Optional<DetectedObjectState> mDetectedNoteForPickup = Optional.empty();
 
-    public void addSubsystems(Drive drive, Shooter shooter, Indexer indexer, Intake intake) {
+    public void addSubsystems(Drive drive, Shooter shooter, Indexer indexer, Intake intake, Vision vision) {
         mDrive = Optional.of(drive);
         mShooter = Optional.of(shooter);
         mIndexer = Optional.of(indexer);
         mIntake = Optional.of(intake);
+        mVision = Optional.of(vision);
     }
 
     public void periodic() {
@@ -354,6 +357,13 @@ public ShootingState getShootingState() {
         return mShootingState;
     }
 
+    public Optional<Boolean> detectorCameraIsConnected(int index) {
+        if (mVision.isEmpty()) {
+            return Optional.of(false);
+        }
+        return mVision.get().detectorCameraIsConnected(index);
+    }
+
     public void setScoringMode(ScoringMode scoringMode) {
         this.mScoringMode = scoringMode;
     }

diff --git a/src/main/java/com/team1701/robot/subsystems/leds/LED.java b/src/main/java/com/team1701/robot/subsystems/leds/LED.java
@@ -1,5 +1,7 @@
 package com.team1701.robot.subsystems.leds;
 
+import java.util.Optional;
+
 import com.team1701.lib.drivers.leds.LEDController;
 import com.team1701.robot.Configuration;
 import com.team1701.robot.states.RobotState;
@@ -17,9 +19,10 @@ public class LED extends SubsystemBase {
     private static final int kTopLEDsPerRow = kTopLEDCount / kTopLEDsRowCount;
     private static final int kCylonFrequency = 16;
     private static final int kStrobeFrequency = 10;
+    private Optional<Boolean> detectorCamConnected;
 
     private final LEDController mLEDController;
-    private final RobotState mRobotState;
+    public final RobotState mRobotState;
 
     public LED(RobotState robotState) {
         mLEDController = new LEDController(0, kTopLEDCount);
@@ -28,23 +31,24 @@ public LED(RobotState robotState) {
 
     @Override
     public void periodic() {
+        detectorCamConnected = mRobotState.detectorCameraIsConnected(0);
         if (DriverStation.isDisabled()) {
             setDisabledLEDStates();
         } else if (mRobotState.hasNote()) {
             if (mRobotState.getShootingState().isActive) {
                 setScoringLEDStates();
             } else if (!mRobotState.hasLoadedNote()) {
-                setStrobe(LEDColors.kIdleHasNote);
+                setStrobe(LEDColors.kShooterProgressBar);
             } else if (mRobotState.isSpeakerMode() && !mRobotState.inOpponentWing()) {
                 setSpeakerRangeLEDStates();
             } else {
-                mLEDController.setAll(LEDColors.kIdleHasNote);
+                mLEDController.setAll(LEDColors.kIdleHasNote, detectorCamConnected);
             }
         } else {
             if (mRobotState.getDetectedNoteForPickup().isPresent()) {
-                mLEDController.setAll(LEDColors.kSeesNote);
+                mLEDController.setAll(LEDColors.kSeesNote, detectorCamConnected);
             } else {
-                mLEDController.setAll(LEDColors.kIdleNoNote);
+                mLEDController.setAll(LEDColors.kIdleNoNote, detectorCamConnected);
             }
         }
         mLEDController.update();
@@ -67,17 +71,17 @@ private void setDisabledLEDStates() {
             cylonColumn = (kTopLEDsPerRow - 1) * 2 - cylonColumn;
         }
 
-        mLEDController.setAll(Color.kBlack);
+        mLEDController.setAll(Color.kBlack, detectorCamConnected);
         for (var row = 0; row < kTopLEDsRowCount; row++) {
             var rowStart = row * kTopLEDsPerRow;
             var rowEnd = rowStart + kTopLEDsPerRow - 1;
             var column = row % 2 == 0 ? rowStart + cylonColumn : rowEnd - cylonColumn;
-            mLEDController.set(column, color);
+            mLEDController.set(column, color, detectorCamConnected);
             if (column > rowStart) {
-                mLEDController.set(column - 1, color, 0.25);
+                mLEDController.set(column - 1, color, 0.25, detectorCamConnected);
             }
             if (column < rowEnd) {
-                mLEDController.set(column + 1, color, 0.25);
+                mLEDController.set(column + 1, color, 0.25, detectorCamConnected);
             }
         }
     }
@@ -113,7 +117,7 @@ private void setSpeakerRangeLEDStates() {
         mLEDController.setRange(0, kTopLEDCount, LEDColors.kIdleHasNoteSpeakerFar);
         for (var row = 0; row < kTopLEDsRowCount; row++) {
             var start = unlitLEDsPerSide + row * kTopLEDsPerRow;
-            mLEDController.setRange(start, start + litLEDs, LEDColors.kIdleHasNote);
+            mLEDController.setRange(start, start + litLEDs, LEDColors.kShooterProgressBar);
         }
     }
 }
diff --git a/src/main/java/com/team1701/robot/subsystems/leds/LEDColors.java b/src/main/java/com/team1701/robot/subsystems/leds/LEDColors.java
@@ -6,11 +6,13 @@ public class LEDColors {
     public static final Color kDisabledRed = Color.kRed;
     public static final Color kDisabledBlue = Color.kBlue;
     public static final Color kIdleHasNote = Color.kBlue;
+    public static final Color kShooterProgressBar = Color.kPurple;
     public static final Color kIdleHasNoteSpeakerFar = Color.kBlack;
     public static final Color kIdleNoNote = Color.kRed;
     public static final Color kSeesNote = Color.kOrangeRed;
     public static final Color kShooting = Color.kWhite;
-    public static final Color kShootingValid = Color.kGreen;
+    public static final Color kShootingValid = Color.kYellow;
     public static final Color kShootingInvalid = Color.kBlack;
     public static final Color kClimbing = Color.kWhite;
+    public static final Color kLimelightDisabled = Color.kOrange;
 }
diff --git a/src/main/java/com/team1701/robot/subsystems/vision/Vision.java b/src/main/java/com/team1701/robot/subsystems/vision/Vision.java
@@ -163,4 +163,11 @@ public void simulationPeriodic() {
         mVisionSim.get().update(mRobotState.getPose2d());
         Logger.recordOutput("Vision/SimPose", mVisionSim.get().getDebugField().getRobotPose());
     }
+
+    public Optional<Boolean> detectorCameraIsConnected(int index) {
+        if (mDetectorCameras.size() >= index + 1) {
+            return Optional.of(mDetectorCameras.get(index).isConnected());
+        }
+        return Optional.empty();
+    }
 }
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,3 @@
		package com.team1701.lib.drivers.leds;

		public enum mRobotState {}