ChainLynxRobotics · jovapo · Mar 16, 2026
diff --git a/docs/FuelVisionSetup.md b/docs/FuelVisionSetup.md
@@ -0,0 +1,95 @@
+# FuelVision — PhotonVision ML Setup for OrangePi 5
+
+Camera: **Arducam OV9782**, 1MP 1280×800 global shutter, 70°(H) M12 low-distortion lens.
+Pipeline name in PhotonVision dashboard: **`fuelCam`** (must match `FuelVisionConstants.kCameraName`).
+
+---
+
+## Recommended Model: Use the Built-In PhotonVision Fuel Model
+
+PhotonVision v2026.2.1+ ships with an official Fuel detection model contributed by **Team 2826 (Wave Robotics)**. It is bundled in the PhotonVision JAR and extracted automatically on first boot — no manual download needed if you flash the latest image.
+
+- Architecture: YOLOv11, 640×640 input
+- Training data: ~2,600 augmented images from 60 original photos
+- Expected performance on OrangePi 5 NPU: 30+ FPS
+
+**This is the right starting point.** Try it before considering a custom model.
+
+---
+
+## Model Options
+
+### Option 1 — Official PhotonVision Fuel model (bundled, recommended)
+Already on the device after flashing. Select it in the PhotonVision dashboard under the Object Detection pipeline.
+
+### Option 2 — Wave Robotics YOLOv11 standalone release
+Same model as above but available separately if you need to update without reflashing.
+- Chief Delphi thread: https://www.chiefdelphi.com/t/introducing-wave-robotics-yolov11-model-for-rebuilt/512701
+
+### Option 3 — Popcorn Penguins (Team 6238) YOLOv11 model
+An independently trained alternative — worth testing if the official model gives too many false positives in your venue.
+- Chief Delphi thread: https://www.chiefdelphi.com/t/popcorn-penguins-yolov11-rebuilt-vision-model/514496
+ashboard
+
+**OrangePi 5 requirements:**
+- Format: `.rknn` only (uses the RK3588 6-TOPS NPU via PhotonVision's RKNN JNI wrapper)
+- Must be quantized (int8) — non-quantized models will not run
+- Supported architectures: YOLOv5, YOLOv5u, YOLOv8, YOLOv11 (640×640)
+
+**Benchmark (OrangePi 5, COCO 2017):**
+| Model    | Inference | mAP    |
+|----------|-----------|--------|
+| YOLOv5   | ~15 ms    | 0.2243 |
+| YOLOv5u  | ~16 ms    | 0.2745 |
+| YOLOv8   | ~17 ms    | 0.3051 |
+| YOLOv11  | ~23 ms    | 0.3251 |
+
+
+## Pre-Downloading Models for Competition (Offline Use)
+
+Models live on the OrangePi at:
+```
+/opt/photonvision/photonvision_config/models/
+```
+
+Each model needs two files:
+```
+fuel-640-640-yolov11s.rknn
+fuel-640-640-yolov11s-labels.txt
+```
+
+**To pre-load a model without internet at the venue:**
+1. Download the `.rknn` and `-labels.txt` files at home
+2. Copy them to the OrangePi via SCP/FileZilla over USB or the robot network:
+   ```
+   scp fuel-640-640-yolov11s.rknn pi@10.82.48.11:/opt/photonvision/photonvision_config/models/
+   scp fuel-640-640-yolov11s-labels.txt pi@10.82.48.11:/opt/photonvision/photonvision_config/models/
+   ```
+3. Restart PhotonVision — the model will appear in the dashboard
+
+**Alternative — export/import full config:**
+The PhotonVision dashboard can export the entire `photonvision_config/` directory as a ZIP (Settings → Export). Import it on another device to replicate all pipelines and models at once. Useful for swapping or cloning OrangePi units at competition.
+
+---
+
+## Deploying a New PhotonVision Version
+
+Latest release: https://github.com/PhotonVision/photonvision/releases
+
+Flash the OrangePi image rather than upgrading the JAR when possible — this ensures the RKNN runtime and JNI libraries match the PhotonVision version.
+
+---
+
+## Simulation Note
+
+The real camera ML pipeline cannot be simulated — PhotonVision's `VisionSystemSim` only renders AprilTag scenes and does not run ML models on synthetic frames. `FuelVision.java` uses Maple Sim ground-truth game piece positions in simulation instead, which is the standard approach for ML subsystem testing in FRC.
+
+---
+
+## TODO After Physical Camera Mounting
+
+- [ ] Measure actual camera offset and update `FuelVisionConstants.kRobotToCamera` (X forward, Y left, Z up from robot center, plus roll/pitch/yaw)
+- [ ] Calibrate camera in PhotonVision dashboard (required for ML pipeline)
+- [ ] Confirm pipeline name matches `"fuelCam"` in PhotonVision
+- [ ] Verify `kHorizHalfFOVDegrees` / `kVertHalfFOVDegrees` match actual lens (spec says 70° H)
+- [ ] Test detection at various field lighting conditions and tune confidence threshold in PhotonVision if needed
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -17,12 +17,13 @@
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.ironmaple.simulation.SimulatedArena;
 
 @Logged
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
-  private final RobotContainer m_robotContainer;
+  @Logged private final RobotContainer m_robotContainer;
   private final boolean enableLogging = true;
   private final boolean logToAdvantageScope = true;
 
@@ -74,6 +75,7 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
+    if (RobotBase.isSimulation()) SimulatedArena.getInstance().resetFieldForAuto();
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     if (m_autonomousCommand != null) {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -47,6 +47,7 @@
 import frc.robot.subsystems.Shooter.ShooterConstants;
 import frc.robot.subsystems.Shooter.ShooterLUT;
 import frc.robot.subsystems.Swerve.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Vision.FuelVision;
 import frc.robot.subsystems.Vision.Vision;
 import frc.robot.subsystems.climber.Climber;
 import frc.robot.utils.PointingUtil;
@@ -55,6 +56,7 @@
 import java.util.function.Supplier;
 import org.ironmaple.simulation.SimulatedArena;
 
+@Logged
 public class RobotContainer {
 
   private boolean doDriving;
@@ -85,6 +87,10 @@ public class RobotContainer {
   @Logged
   public final Vision vision = new Vision(drivetrain::passVisionPose, drivetrain::getSimPose);
 
+  // TODO: replace getSimPose with the real robot pose getter once the camera is mounted
+  // (e.g., () -> drivetrain.getState().Pose)
+  @Logged public final FuelVision fuelVision = new FuelVision(drivetrain::getSimPose);
+
   @Logged
   private final IntakeSubsystem intake =
       new IntakeSubsystem(new TalonFX(15, kCanBusRio), new TalonFX(16, kCanBusRio));

diff --git a/src/main/java/frc/robot/subsystems/Vision/FuelVision.java b/src/main/java/frc/robot/subsystems/Vision/FuelVision.java
@@ -0,0 +1,187 @@
+package frc.robot.subsystems.Vision;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.subsystems.Vision.FuelVisionConstants.*;
+import static frc.robot.subsystems.Vision.VisionConstants.*;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.epilogue.NotLogged;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.Supplier;
+import org.ironmaple.simulation.SimulatedArena;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+@Logged
+public class FuelVision extends SubsystemBase {
+
+  private final PhotonCamera camera;
+  private final Supplier<Pose2d> getRobotPose;
+
+  private final List<FuelEntry> fuelMap = new ArrayList<>();
+
+  private record FuelEntry(Translation2d position, double timestampSecs) {}
+
+  public FuelVision(Supplier<Pose2d> getRobotPose) {
+    this.camera = new PhotonCamera(kCameraName);
+    this.getRobotPose = getRobotPose;
+  }
+
+  @Override
+  public void periodic() {
+    if (RobotBase.isSimulation()) {
+      Pose2d robotPose = getRobotPose.get();
+      double headingDeg = robotPose.getRotation().getDegrees();
+      for (Pose3d piece : SimulatedArena.getInstance().getGamePiecesArrayByType("Fuel")) {
+        Translation2d fieldXY = piece.getTranslation().toTranslation2d();
+
+        // Distance filter
+        if (fieldXY.getDistance(robotPose.getTranslation()) > kMaxDetectionDistanceMeters) continue;
+
+        // Horizontal FOV: bearing to piece relative to robot heading (camera faces forward)
+        double bearingDeg =
+            Math.toDegrees(
+                Math.atan2(fieldXY.getY() - robotPose.getY(), fieldXY.getX() - robotPose.getX()));
+        double relAngle = ((bearingDeg - headingDeg) % 360 + 360) % 360;
+        if (relAngle > 180) relAngle = 360 - relAngle;
+        if (relAngle > kHorizHalfFOVDegrees) continue;
+
+        mergeIntoMap(fieldXY);
+      }
+      ageOutMap();
+      return;
+    }
+
+    Pose2d robotPose = getRobotPose.get();
+
+    for (var result : camera.getAllUnreadResults()) {
+      for (PhotonTrackedTarget target : result.getTargets()) {
+        // Airborne filter: skip if pitch is above the horizon threshold
+        if (target.getPitch() > kMaxPitchDegrees) continue;
+
+        // Floor projection: find field-frame XY where the bearing ray hits z = kFuelRadius
+        Pose3d cameraPose = new Pose3d(robotPose).transformBy(kRobotToCamera);
+        double yawRad = Math.toRadians(target.getYaw());
+        double pitchRad = Math.toRadians(target.getPitch());
+
+        // Unit bearing vector in camera NWU frame (+X forward, +Y left, +Z up)
+        Translation3d dirCamera =
+            new Translation3d(
+                Math.cos(pitchRad) * Math.cos(yawRad),
+                Math.cos(pitchRad) * Math.sin(yawRad),
+                Math.sin(pitchRad));
+
+        Translation3d dirField = dirCamera.rotateBy(cameraPose.getRotation());
+
+        // Ray must point downward in field frame to intersect the floor
+        double dirFieldZ = dirField.getZ();
+        if (dirFieldZ >= 0) continue;
+
+        double t = (kFuelRadiusMeters - cameraPose.getZ()) / dirFieldZ;
+        if (t <= 0) continue;
+
+        Translation2d fieldXY =
+            new Translation2d(
+                cameraPose.getX() + t * dirField.getX(), cameraPose.getY() + t * dirField.getY());
+
+        // Distance filter
+        if (fieldXY.getDistance(robotPose.getTranslation()) > kMaxDetectionDistanceMeters) continue;
+
+        // Field bounds filter
+        if (fieldXY.getX() < 0
+            || fieldXY.getX() > kFieldWidth.in(Meters)
+            || fieldXY.getY() < 0
+            || fieldXY.getY() > kFieldHeight.in(Meters)) continue;
+
+        mergeIntoMap(fieldXY);
+      }
+    }
+
+    ageOutMap();
+  }
+
+  private void mergeIntoMap(Translation2d fieldXY) {
+    for (int i = 0; i < fuelMap.size(); i++) {
+      if (fuelMap.get(i).position().getDistance(fieldXY) < kMergeRadiusMeters) {
+        // Blend toward new reading, refresh timestamp
+        double blendedX = fuelMap.get(i).position().getX() * 0.7 + fieldXY.getX() * 0.3;
+        double blendedY = fuelMap.get(i).position().getY() * 0.7 + fieldXY.getY() * 0.3;
+        fuelMap.set(
+            i, new FuelEntry(new Translation2d(blendedX, blendedY), Timer.getFPGATimestamp()));
+        return;
+      }
+    }
+    fuelMap.add(new FuelEntry(fieldXY, Timer.getFPGATimestamp()));
+  }
+
+  private void ageOutMap() {
+    double now = Timer.getFPGATimestamp();
+    fuelMap.removeIf(e -> now - e.timestampSecs() > kMapAgeSecs);
+  }
+
+  // ---- Public API ----
+
+  /** All currently tracked fuel positions. */
+  @NotLogged
+  public List<Translation2d> getFuelMap() {
+    return fuelMap.stream().map(FuelEntry::position).toList();
+  }
+
+  /** Nearest fuel to the given pose; empty if map has no entries. */
+  public Optional<Translation2d> getNearestFuel(Pose2d robotPose) {
+    return fuelMap.stream()
+        .map(FuelEntry::position)
+        .min(Comparator.comparingDouble(p -> p.getDistance(robotPose.getTranslation())));
+  }
+
+  /** Removes map entries near the given point (call after intake contact). */
+  public void removeFuelNear(Translation2d point) {
+    fuelMap.removeIf(e -> e.position().getDistance(point) < kMergeRadiusMeters);
+  }
+
+  /** Wipes the entire map (e.g., at auto start). */
+  public void clearFuelMap() {
+    fuelMap.clear();
+  }
+
+  // ---- Epilogue logging ----
+
+  public Pose2d[] logFuelMap() {
+    return fuelMap.stream()
+        .map(e -> new Pose2d(e.position(), new Rotation2d()))
+        .toArray(Pose2d[]::new);
+  }
+
+  public Pose3d[] logFuelMap3d() {
+    return fuelMap.stream()
+        .map(
+            e ->
+                new Pose3d(
+                    e.position().getX(), e.position().getY(), kFuelRadiusMeters, new Rotation3d()))
+        .toArray(Pose3d[]::new);
+  }
+
+  public int logFuelCount() {
+    return fuelMap.size();
+  }
+
+  public double logNearestFuelMeters() {
+    Translation2d robotXY = getRobotPose.get().getTranslation();
+    return fuelMap.stream()
+        .mapToDouble(e -> e.position().getDistance(robotXY))
+        .min()
+        .orElse(Double.NaN);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Vision/FuelVisionConstants.java b/src/main/java/frc/robot/subsystems/Vision/FuelVisionConstants.java
@@ -0,0 +1,36 @@
+package frc.robot.subsystems.Vision;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+public class FuelVisionConstants {
+  public static final String kCameraName = "fuelCam";
+
+  // TODO: measure and fill in the actual robot-to-camera offset after mounting.
+  // X is forward, Y is left, Z is up from robot center; negative pitch tilts the camera down.
+  // Currently placed at front-center of robot frame, 0.5m high, aimed 30° down.
+  public static final Transform3d kRobotToCamera =
+      new Transform3d(
+          Inches.of(14.0),
+          Meters.of(0.0),
+          Meters.of(0.5),
+          new Rotation3d(Degrees.zero(), Degrees.of(-30), Degrees.zero()));
+
+  // Arducam OV9782 with 70°(H) M12 lens, 1280x800 sensor.
+  // HFOV = 70° (from spec sheet). VFOV derived: 2*atan(tan(35°)*800/1280) ≈ 47.3°
+  public static final double kHorizHalfFOVDegrees = 35.0; // half of 70° HFOV
+  public static final double kVertHalfFOVDegrees = 23.7; // half of ~47.3° VFOV
+
+  // Fuel ball radius in meters; used as the floor-plane intersection height (z = ball center).
+  public static final double kFuelRadiusMeters = 0.0762;
+  // Discard floor projections farther than this distance from the robot.
+  public static final double kMaxDetectionDistanceMeters = 4.0;
+  // Two detections within this radius are treated as the same game piece.
+  public static final double kMergeRadiusMeters = 0.25;
+  // Map entries not refreshed within this number of seconds are pruned.
+  public static final double kMapAgeSecs = 1.0;
+  // If a target's pitch exceeds this, the piece is likely airborne. Tune based on camera tilt.
+  public static final double kMaxPitchDegrees = 5.0;
+}