diff --git a/doc/README.md b/doc/README.md index a03af0b..471d9ff 100644 --- a/doc/README.md +++ b/doc/README.md @@ -25,6 +25,7 @@ project front door; this page is the working table of contents. | [recommendations/FRC-Product-Watch-2026-2027.md](recommendations/FRC-Product-Watch-2026-2027.md) | Purchasing and evaluation recommendations for notable REBUILT products and BIOCORE-era technology. | | [recommendations/Pose-Odometry-Vision-Integration.md](recommendations/Pose-Odometry-Vision-Integration.md) | Architecture review and migration recommendations for precise, reliable, efficient localization. | | [recommendations/Top-Team-Common-Practices.md](recommendations/Top-Team-Common-Practices.md) | Evidence-backed engineering, software, practice, scouting, and competition routines common to high-performing teams. | +| [recommendations/PhotonVision-Camera-Selection.md](recommendations/PhotonVision-Camera-Selection.md) | Camera, connector, mounting, and test recommendations for reliable PhotonVision input. | ## New User Bring-Up Path @@ -63,6 +64,8 @@ project front door; this page is the working table of contents. [recommendations/Pose-Odometry-Vision-Integration.md](recommendations/Pose-Odometry-Vision-Integration.md) - Common practices of high-performing teams: [recommendations/Top-Team-Common-Practices.md](recommendations/Top-Team-Common-Practices.md) +- PhotonVision camera selection and connection reliability: + [recommendations/PhotonVision-Camera-Selection.md](recommendations/PhotonVision-Camera-Selection.md) - Pose buffer internals: [RBSI-PoseBuffer.md](RBSI-PoseBuffer.md) - AdvantageScope pit-debug layout: diff --git a/doc/recommendations/PhotonVision-Camera-Selection.md b/doc/recommendations/PhotonVision-Camera-Selection.md new file mode 100644 index 0000000..5c43e92 --- /dev/null +++ b/doc/recommendations/PhotonVision-Camera-Selection.md @@ -0,0 +1,421 @@ +# PhotonVision Camera Selection and Reliability + +**Research snapshot:** August 29, 2026 + +**Scope:** camera modules, optics, connectors, and camera-to-coprocessor links +for PhotonVision. Coprocessor selection and integrated vision appliances are +outside the main comparison except where a host port can masquerade as a +camera failure. + +## Recommendation In Brief + +Az-RBSI teams should standardize on the **Thriftiest Cam as the default +AprilTag camera**. It costs the same as the commonly used Arducam B0332 OV9281 +at the time of this review, uses the same proven OV9281 global-shutter sensor, +and addresses the most frequently reported FRC failure point with an enclosed +board and locking USB-C cable. + +Use the **2 MP ThriftyCam selectively** where testing proves that longer-range +tag detection materially improves the robot. It has a stronger enclosure, +locking connector, interchangeable lenses, and more pixels, but costs more and +consumes substantially more processing and USB bandwidth. + +Do not purchase more JST/pigtail-style Arducam OV9281 or OV2311 modules for a +competition robot. Existing units remain useful on benches and prototypes. +Replacing them with another board using the same small camera-side connector +does not address the team's reported failure mode. + +A practical starting configuration is: + +- two Thriftiest Cams for complementary AprilTag coverage; +- one additional Thriftiest Cam as a tested, separately calibrated spare; +- direct, retained USB connections to known-good host ports; +- rigid, guarded camera mounts with cable strain relief; and +- explicit camera-health logging plus manual robot-control fallbacks. + +If long-range testing demonstrates a gap, replace the forward-facing unit with +one 2 MP ThriftyCam rather than upgrading every camera. + +## The Important Distinction: Sensor Versus Camera Assembly + +“OV9281” names the image sensor. It does not specify the board, USB bridge, +connector, cable, enclosure, lens, host port, driver, or power source. + +That distinction explains the available evidence: + +- Teams often report good tag-detection performance from Arducam OV9281s. +- Other teams report “Camera Lost,” intermittent video, loose camera-side + connectors, and recovery only after unplugging or power cycling. +- Similar reports have been traced to bad Orange Pi or Rubik Pi USB ports, + bandwidth, power, or enumeration rather than a failed sensor. +- The Thriftiest Cam also uses an OV9281, but packages it with a locking USB-C + connection and robot-oriented enclosure. + +The team's frustration is therefore credible without implying that OV9281 is +a poor vision sensor. The stronger diagnosis is that the common low-cost +Arducam assembly and ordinary USB link are insufficiently retained and +observable for the team's competition environment. + +Reports of loose Arducam connectors and intermittent camera loss appear in +[this 2025 Arducam thread](https://www.chiefdelphi.com/t/arducam-sporadically-displaying-camera-lost/496411) +and [this 2026 OV2311 thread](https://www.chiefdelphi.com/t/arducam-ov2311-connectivity-issues-any-tips/515886). +A separate case that initially looked like a camera problem was ultimately +attributed to a bad Orange Pi port. [Orange Pi port diagnosis](https://www.chiefdelphi.com/t/photon-vision-cameras-are-fliping-180-degrees-randomly/460501) + +## Selection Criteria + +For AprilTag localization on a moving FRC robot, evaluate the complete camera +assembly against these priorities, in order: + +1. **Connection retention and strain relief.** A sharper image has no value + after the camera disconnects. +2. **Known PhotonVision/V4L2 behavior.** A UVC label alone does not prove every + mode and control works correctly with CSCore on the selected host. +3. **Global shutter.** It substantially reduces geometric distortion from + robot motion compared with rolling-shutter webcams. +4. **Rigid and repeatable mounting.** Pose estimation assumes the calibrated + camera intrinsics and robot-to-camera transform remain true. +5. **Useful field of view and resolution.** More pixels extend detection range + but consume compute, bandwidth, exposure, and calibration effort. +6. **Replaceability.** A competition spare needs a known cable, mount, USB + port, calibration, settings backup, and swap procedure. +7. **Price.** Compare the usable assembly—including case, lens, retained cable, + mount, and spare—not the bare sensor board. + +PhotonVision recommends global shutter to reduce motion blur and advises using +the highest AprilTag resolution that still maintains reasonable frame rate. +It also notes that multi-camera limits are normally CPU and USB bandwidth, not +an arbitrary software camera count. [PhotonVision camera tuning](https://docs.photonvision.org/en/latest/docs/pipelines/input.html) +[PhotonVision hardware selection](https://docs.photonvision.org/en/latest/docs/hardware/selecting-hardware.html) + +## Camera Comparison + +Prices are vendor list prices observed on August 29, 2026, before shipping and +tax. Team reports are valuable but not controlled endurance studies. + +| Camera | Camera and link | Price | Reliability evidence | Ease of use | Recommendation | +| --- | --- | ---: | --- | --- | --- | +| Thriftiest Cam TTB-0370 | 1 MP monochrome OV9281, global shutter, USB 2.0, locking USB-C cable, molded case | $49.99 | Limited but encouraging 2026 beta and Championship use; reported no failures in the cited deployments | Familiar PhotonVision behavior, included retained cable and mountable enclosure | **Default AprilTag camera** | +| ThriftyCam TTB-0241 | 2 MP monochrome OG02B1B, global shutter, USB 3.0, locking USB-C, aluminum case, M12 lens | $120 | Multiple successful team deployments; isolated camera/USB-C deaths and host compatibility issues also reported | Mechanically strong and flexible optics; higher compute and USB tuning burden | **Use for demonstrated long-range need** | +| Arducam B0332/UC-844-style OV9281 | 1 MP monochrome OV9281, global shutter, USB 2.0 UVC, small board/pigtail connection | $49.99 | Widely used with good optical performance, but repeated loose-connector and intermittent-loss reports match Az-RBSI's experience | Initially plug-and-play; retention, exposed-board mounting, and recovery become team work | **No new competition purchases** | +| Raspberry Pi CSI OV9281 | 1 MP monochrome OV9281, global shutter, MIPI CSI ribbon cable | about $42 | Can avoid USB bandwidth/enumeration, but evidence is more integration-specific than competition-wide | Raspberry Pi only in official PhotonVision support; boot overlay and fragile FFC handling add service risk | **Conditional bench experiment, not default** | +| Generic UVC webcam | Usually color rolling shutter over USB | roughly $20–$70 | Quality and Linux controls vary by exact model; PhotonVision explicitly excludes Logitech cameras from current general support | Cheap for experiments, but model validation and motion blur undermine standardization | **Driver view or prototype only** | + +### Thriftiest Cam: best fit for the stated problem + +The [Thriftiest Cam product page](https://www.thethriftybot.com/products/thriftiest-cam) +lists a $49.99 price, OV9281 global-shutter sensor, 1280 x 800 resolution, +USB 2.0 connection, molded case, and included locking cable. + +The strongest field report is still limited in sample size but directly +relevant. Teams 4451 and 4864 beta-tested the camera with PhotonVision on +Rubik Pi and Orange Pi hardware; the author reported that a new-student team +had no setup or calibration difficulty and replaced its Arducam OV9281s. Team +3476 used two at Championship with no reported failures. The same testing found +similar AprilTag performance to Arducam OV9281s, which is expected because the +sensor class is the same. The claimed advantage was connector and package +quality, not a magical image improvement. [Thriftiest Cam field report](https://www.chiefdelphi.com/t/the-thrifty-bot-2025-2026-product-releases-updates/505971?page=19) + +That evidence appears in a vendor release thread, and contributors disclosed +product-testing relationships. It is useful operational evidence, not an +independent reliability study. Even with that limitation, this is the best +one-for-one response to Az-RBSI's problem: it preserves the known sensor +behavior and processing load while replacing the connection most often +implicated in dropouts. + +**Caution:** the production product only began shipping in July 2026. The +evidence does not yet cover several full seasons or thousands of team-events. +Adopt it with an acceptance test and a spare, not blind confidence. + +### 2 MP ThriftyCam: robust premium option + +The [ThriftyCam product page](https://www.thethriftybot.com/products/thriftycam) +lists a $120 price, 1600 x 1304 monochrome global-shutter sensor, USB 3.0, +machined aluminum case, included locking cable, and an 80-degree replaceable +M12 lens. Higher resolution can improve long-distance tag detection, and lens +options allow the team to trade field of view for pixels on target. + +Team reports include successful multi-camera operation. Team 3476 used four +USB 3.0 ThriftyCams across three 2026 events, while a HighTide discussion +reported a Rubik Pi and ThriftyCam arrangement that was solid across multiple +robots. [ThriftyCam/Thriftiest Cam field report](https://www.chiefdelphi.com/t/the-thrifty-bot-2025-2026-product-releases-updates/505971?page=19) +[2026 HighTide vision discussion](https://www.chiefdelphi.com/t/team-4414-hightide-2026-tech-binder-ripcurrent/519602?page=6) + +It is not failure-proof. One 2025 team reported a mysterious death and a second +failure apparently at the USB-C port; the vendor replaced the first unit. +Another 2026 team found that its cameras worked on USB 2 but not correctly on +the USB 3 port of three Orange Pi 5 Pro units, illustrating that a host/driver +interaction can dominate the camera specification. [2025 ThriftyCam experience](https://www.chiefdelphi.com/t/thrifty-camera-resources/498482) +[Orange Pi 5 Pro compatibility report](https://www.chiefdelphi.com/t/thrifty-cam-with-orange-pi-5-pro/515006) + +Do not pay for 2 MP everywhere by default. A 2 MP image has approximately twice +the pixels of a 1280 x 800 image, and uncompressed high-rate streams can become +the limiting resource. First prove that the 1 MP camera misses required tags at +the team's actual distances, angles, lighting, speed, and mounting height. + +### Arducam USB OV9281: good sensor, weak competition package + +The [Arducam B0332 product page](https://www.arducam.com/arducam-100fps-global-shutter-usb-camera-board-1mp-720p-ov9281-uvc-webcam-module-with-low-distortion-m12-lens-without-microphones-for-computer-laptop-android-device-and-raspberry-pi.html) +lists a $49.99 price, 1280 x 800 monochrome global shutter, native UVC support, +USB 2.0, and a 70-degree lens. Teams have achieved strong PhotonVision results +with one or two of these cameras, so replacing them should not be justified as +an AprilTag-performance upgrade. + +The reliability pattern is the problem. Chief Delphi reports describe the +small camera-board connector loosening, camera streams disappearing while the +coprocessor remains alive, and temporary fixes using hot glue or soldered +connections. Az-RBSI's mid-match cutouts are consistent with those reports. +Hot glue can be a diagnostic or emergency retention measure, but it is not a +good fleet standard when a price-equivalent, purpose-built locking alternative +now exists. + +The team should: + +- stop buying this package for competition; +- mark existing cameras “practice/prototype only” unless they pass the new + endurance test; +- retain a few as known-load diagnostic devices; and +- avoid assuming a new OV2311 board with the same connector style fixes the + mechanical problem. + +### CSI cameras: fewer USB problems, different fragility + +An OV9281 connected through a Raspberry Pi CSI port avoids a USB bridge and +some USB enumeration/bandwidth problems. That can look attractive after USB +failures, but it trades them for a thin FFC cable, delicate board connectors, +boot configuration, restricted coprocessor choice, and more cumbersome pit +replacement. + +PhotonVision currently supports MIPI CSI only on Raspberry Pi and documents +that some cameras require a boot overlay; an incorrect overlay or reversed +ribbon looks exactly like a missing camera. [PhotonVision Raspberry Pi camera configuration](https://docs.photonvision.org/en/latest/docs/camera-specific-configuration/picamconfig.html) +[PhotonVision camera troubleshooting](https://docs.photonvision.org/en/latest/docs/troubleshooting/camera-troubleshooting.html) + +Use CSI only if the team is deliberately standardizing on Raspberry Pi, +develops an enclosed and retained ribbon solution, and validates replacement +and recovery. It is not the recommended reaction to the current failures. + +### Generic webcams: false economy for primary localization + +Rolling-shutter webcams can work for a driver stream, stationary development, +or simple color detection. They are a poor Az-RBSI default for AprilTag pose on +a moving robot because motion can distort the observed corners, model-specific +controls vary, housings are not designed for robot mounting, and PhotonVision's +current hardware guidance specifically lists Logitech cameras as unsupported. + +The cost saved at purchase is easily consumed by calibration, compatibility, +mounting, and debugging. Do not standardize on “any UVC webcam”; qualify an +exact manufacturer, model, hardware revision, mode, cable, host, and +PhotonVision release. + +## Reliability Diagnosis Before Replacing Hardware + +Do not classify every yellow stream or missing estimate as a dead camera. +Capture the failure state before rebooting. + +| Observation | More likely fault domain | Immediate checks | +| --- | --- | --- | +| PhotonVision host and NetworkTables disappear | Coprocessor power, storage, network, or whole-host crash | Regulator voltage/current, Ethernet, boot log, temperature, storage health | +| PhotonVision stays live but one camera becomes “Camera Lost” | Camera cable, camera connector, one host port, V4L2/CSCore, or USB power | Swap only the cable, then only the port; inspect `dmesg` and `v4l2-ctl`; preserve timestamps | +| Multiple cameras fail together | Shared USB root hub/bandwidth, host power, PhotonVision process, or coprocessor | USB topology, aggregate modes, regulator, process logs | +| Failure follows impact or cable motion | Connector retention, cable strain, cracked board/port, or mount contact | Wiggle test while logging, connector inspection, cable continuity | +| Camera is absent or mismatched only at boot | Enumeration, physical-port matching, duplicate identity, or startup timing | Fixed port map, strict matching, camera detail/mismatch banner, cold-boot repetitions | +| Image is present but tags disappear | Exposure, focus, lens contamination, glare/shadow, occlusion, resolution, or calibration | Raw frame, exposure/gain, focus lock, lens cleaning, field lighting, calibration | +| Estimates jump while frames remain healthy | Calibration, robot-to-camera transform, tag ambiguity, timing, or estimator filtering | Calibration residuals, rigid mount, timestamp/latency logs, per-observation rejection reason | + +The 2026 community reported two Rubik Pi units losing a USB-A port while the +cameras worked after moving ports. That does not prove a systemic Rubik Pi +defect, but it demonstrates why a port swap must precede declaring the camera +dead. [Rubik Pi USB-port report](https://www.chiefdelphi.com/t/rubik-pi-3-losing-usb-ports/517230) + +## Mechanical and Electrical Standard + +The camera installation should be treated like a precision sensor and a +competition electrical connection. + +### Mounting + +- Mount the case rigidly to a stiff chassis or superstructure datum. +- Do not use a compliant “shock mount” if it permits the camera transform to + change under acceleration. +- Guard the lens and connector from game pieces, bumpers, tools, and hands. +- Keep the lens accessible for inspection and cleaning. +- Use a focus lock or witness mark after focusing. +- Add alignment marks so a shifted mount is visible during pit inspection. +- Measure the robot-to-camera transform from real datums; do not rely only on + nominal CAD or a printed mount's assumed dimensions. + +### Cabling + +- Use the supplied locking cable at the camera end. +- Retain the cable near both ends so its mass cannot load either connector. +- Include a small service loop without allowing the cable to whip. +- Use the shortest practical direct cable; avoid adapters and extensions. +- Avoid an unpowered hub. If a hub is unavoidable, qualify the exact powered + model and include it in bandwidth, power-cycle, and vibration tests. +- Label camera identity and physical host port at both ends. +- Place cameras across independent host USB root buses when the hardware and + bandwidth testing support it. + +### Power and host ports + +- Size and validate the regulated coprocessor supply with every camera active. +- Log host undervoltage, reboot, temperature, USB errors, and camera frame age. +- Do not assume connector color proves port bandwidth or root-bus independence; + verify the coprocessor's topology and test the exact configuration. +- Treat a host port that fails substitution testing as unavailable, even if it + works intermittently on the bench. + +## PhotonVision Configuration Standard + +PhotonVision camera configurations are associated with physical USB ports. +Moving a camera to a different port can load the wrong configuration, and +multiple identical cameras require special care. PhotonVision recommends fixed +ports, settings backup, and strict matching where appropriate. +[PhotonVision camera matching](https://docs.photonvision.org/en/latest/docs/quick-start/camera-matching.html) + +For every competition camera: + +1. Assign a semantic name such as `frontLeftTags`, not `Arducam` or `Camera 1`. +2. Record camera serial/asset ID, cable ID, host, physical port, mode, lens, + focus mark, transform, and settings-export filename. +3. Enable strict matching for otherwise indistinguishable USB cameras. +4. Calibrate the exact camera, lens, focus, and processing resolution. +5. Export settings after calibration and after approved field tuning. +6. Keep the processing stream low enough for reliable tag detection and keep + the driver/debug stream at the minimum useful resolution. +7. Freeze the PhotonVision version before an event unless a documented critical + fix is required. + +PhotonVision fixed one OV9281 auto-exposure startup issue in 2026.2.1, while +later users continued to discuss exposure initialization behavior and a +roughly four-second pipeline-switch consequence. This is a reason to run the +exact release and pipeline sequence through repeated cold boots—not a reason +to discard the sensor. [PhotonVision 2026 release discussion](https://www.chiefdelphi.com/t/photonvision-2026-releases-2026-3-4/512436) + +PhotonVision's competition guide recommends exporting settings after field +calibration and avoiding last-minute noncritical upgrades. +[PhotonVision competition practices](https://docs.photonvision.org/en/latest/docs/additional-resources/best-practices.html) + +## Acceptance Test for a Competition Camera + +A camera model is not approved because it worked on a laptop. Test the exact +camera, cable, host port, coprocessor image, PhotonVision version, mode, mount, +and power regulator intended for competition. + +### Bench test + +1. Verify the camera enumerates correctly on 25 consecutive cold boots. +2. Run all cameras concurrently for at least eight hours while logging frames, + disconnects, host temperature, power, and USB errors. +3. Exercise every competition pipeline and resolution transition. +4. Disconnect and reconnect each camera separately and document whether it + recovers without a robot power cycle. +5. Apply a controlled cable-wiggle and connector-load test; no frame loss is + acceptable. +6. Cycle the robot power source through normal shutdown and representative + voltage transients. + +### Robot test + +1. Run at least ten full match simulations with all mechanisms and radios + active. +2. Include hard acceleration, braking, bumps, impacts, and mechanism motion. +3. Compare detection distance, ambiguity, frame age, and accepted-pose rate at + representative field lighting and robot speeds. +4. Inspect the mount, focus witness mark, retained cable, and port after every + block. +5. Review logs before rebooting or changing any failed component. + +### Spare-swap drill + +1. Replace the installed camera and cable using only the pit instructions. +2. Apply the spare's calibration/settings and verify the expected physical + port and transform. +3. Complete the functional vision test and return the robot to ready state. +4. Record the elapsed time and any ambiguous step; revise the instructions + until a trained backup can perform the swap. + +## Az-RBSI Software Recommendations + +Camera reliability should be visible to robot code and logs. Add a per-camera +health record containing at least: + +```text +configured +connected +lastFrameTimestamp +frameAge +pipelineLatency +framesPerSecond +targetCount +lastAcceptedPoseTimestamp +consecutiveInvalidFrames +healthReason +``` + +Derive an overall state such as: + +```text +HEALTHY +NO_TARGETS +STALE_FRAMES +DISCONNECTED +MISCONFIGURED +HIGH_LATENCY +DEGRADED_COVERAGE +``` + +The distinction between `NO_TARGETS` and `DISCONNECTED` is operationally +important. The first may be normal field geometry; the second requires pit +attention and should immediately inhibit any action that assumes current +vision. + +Recommended Az-RBSI changes: + +- publish camera frame age and connection state beside pose quality; +- log accepted and rejected observations by camera with reason; +- alert the driver when coverage is degraded without flooding the dashboard; +- identify which camera and host port failed in the pit health view; +- retain preset/manual aiming and robot-relative driving when vision is lost; +- add a disabled-only camera test that confirms fresh frames and plausible tag + geometry from every installed camera; and +- document the camera asset, port, calibration, settings backup, and transform + in robot configuration. + +These changes complement the architecture in +[Pose, Odometry, and Vision Integration](Pose-Odometry-Vision-Integration.md). + +## Purchasing Recommendation + +For a two-camera competition configuration, purchase: + +| Item | Quantity | Unit price | Extended price | +| --- | ---: | ---: | ---: | +| Thriftiest Cam with locking cable | 3 | $49.99 | $149.97 | +| Team-designed rigid guarded mount | 3 | team-specific | team-specific | +| Labeled spare host-side cable, if the included cable cannot serve that role | 1 | team-specific | team-specific | + +Two cameras are installed and the third is a complete, calibrated spare. This +is effectively the same camera purchase price as three replacement Arducam +OV9281 B0332 units, while providing retained connectors and enclosures. + +Only add a $120 2 MP ThriftyCam after an instrumented comparison demonstrates +that the Thriftiest Cam cannot meet a required tag distance or angle. If that +test succeeds, the likely optimized fleet is one forward 2 MP camera plus one +or more 1 MP wide-coverage cameras, not all 2 MP units. + +## Decision + +Az-RBSI should retain PhotonVision and global-shutter cameras, but retire the +JST/pigtail-style Arducam OV9281 assembly from competition use. + +Adopt the Thriftiest Cam as the default because it solves the reported +mechanical connection problem at no list-price premium while preserving the +team's familiar OV9281 performance and PhotonVision workflow. Treat the 2 MP +ThriftyCam as a measured range upgrade. Pair either choice with rigid mounting, +retained cabling, fixed USB-port mapping, settings backups, health telemetry, +endurance testing, and a practiced spare swap. + +The camera choice reduces failure probability. The installation and diagnostic +standard is what turns that product choice into match reliability. diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index bb0aa70..9ea5afe 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -313,18 +313,18 @@ public void rbsiPeriodic() { } else { final Pose3d[] tagsSeen = new Pose3d[tagIdsSeenThisLoop.size()]; final int[] tagIdsSeen = new int[tagIdsSeenThisLoop.size()]; - int tagCount = 0; + int poseCount = 0; + int idCount = 0; for (int tagId : tagIdsSeenThisLoop) { - tagIdsSeen[tagCount] = tagId; + tagIdsSeen[idCount++] = tagId; Optional tagPose = FieldConstants.aprilTagLayout.getTagPose(tagId); if (tagPose.isPresent()) { - tagsSeen[tagCount] = tagPose.get(); - tagCount++; + tagsSeen[poseCount++] = tagPose.get(); } } Logger.recordOutput( "Vision/TagsSeenThisLoop", - tagCount == tagsSeen.length ? tagsSeen : Arrays.copyOf(tagsSeen, tagCount)); + poseCount == tagsSeen.length ? tagsSeen : Arrays.copyOf(tagsSeen, poseCount)); Logger.recordOutput("Vision/TagIdsSeenThisLoop", tagIdsSeen); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index d6cbb23..0ed6ee6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -86,9 +86,7 @@ public void updateInputs(VisionIOInputs inputs) { List poseObservations = new ArrayList<>(); var megatag1Samples = megatag1Subscriber.readQueue(); - for (int sampleIndex = Math.max(0, megatag1Samples.length - 1); - sampleIndex < megatag1Samples.length; - sampleIndex++) { + for (int sampleIndex = megatag1Samples.length - 1; sampleIndex >= 0; sampleIndex--) { var rawSample = megatag1Samples[sampleIndex]; if (!isValidBotPoseSample(rawSample.value)) continue; @@ -120,12 +118,11 @@ public void updateInputs(VisionIOInputs inputs) { // Used tag IDs used)); + break; } var megatag2Samples = megatag2Subscriber.readQueue(); - for (int sampleIndex = Math.max(0, megatag2Samples.length - 1); - sampleIndex < megatag2Samples.length; - sampleIndex++) { + for (int sampleIndex = megatag2Samples.length - 1; sampleIndex >= 0; sampleIndex--) { var rawSample = megatag2Samples[sampleIndex]; if (!isValidBotPoseSample(rawSample.value)) continue; @@ -153,6 +150,7 @@ public void updateInputs(VisionIOInputs inputs) { // Observation type PoseObservationType.MEGATAG_2, used)); + break; } // Save pose observations to inputs object