diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 162a897..d4b60db 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -54,13 +54,14 @@ public class VisionOdometry extends SubsystemBase { * Breaks down targets into 4 corners and uses the top 2 points * * @return Vision points on the rim of the target in screen space + * @throws VisionObscuredException */ - public ArrayList getTargetPoints() { + public ArrayList getTargetPoints() throws VisionObscuredException { PhotonPipelineResult result = m_camera.getLatestResult(); latency = result.getLatencyMillis(); if(!result.hasTargets()) - return new ArrayList(); + throw new VisionObscuredException(); ArrayList points = new ArrayList<>(); @@ -99,7 +100,7 @@ public class VisionOdometry extends SubsystemBase { ArrayList screenPoints = getTargetPoints(); if(screenPoints.size() < 3) - throw new VisionObscuredException("Not enough vision points available"); + throw new VisionObscuredException("Less than 3 vision points available"); ArrayList points3d = get3dPoints(screenPoints); ArrayList points = topView(points3d); @@ -125,6 +126,7 @@ public class VisionOdometry extends SubsystemBase { public double getLatency() { return latency; } + public boolean getLEDs() { if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false; return true;