From 62df738b55bd1589db10173bae2133a8d14b75b9 Mon Sep 17 00:00:00 2001 From: Joseph Tighe Date: Sun, 15 Mar 2026 21:47:37 -0700 Subject: [PATCH] add FuelVision subsystem for ML game piece detection - FuelVision.java: floor-projection from camera bearing rays on real robot; heading-based FOV filter against Maple Sim ground truth in simulation; merge/age map with 8s timeout; getNearestFuel, removeFuelNear, clearFuelMap - FuelVisionConstants.java: Arducam OV9782 70deg HFOV camera placement (front of robot, 0.5m high, 30deg down), all tunable constants - RobotContainer: instantiate FuelVision, seed sim field with resetFieldForAuto in autonomousInit - docs/FuelVisionSetup.md: PhotonVision model options, OrangePi deployment, offline/competition setup guide --- docs/FuelVisionSetup.md | 95 +++++++++ src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 6 + .../robot/subsystems/Vision/FuelVision.java | 187 ++++++++++++++++++ .../Vision/FuelVisionConstants.java | 36 ++++ 5 files changed, 327 insertions(+), 1 deletion(-) create mode 100644 docs/FuelVisionSetup.md create mode 100644 src/main/java/frc/robot/subsystems/Vision/FuelVision.java create mode 100644 src/main/java/frc/robot/subsystems/Vision/FuelVisionConstants.java diff --git a/docs/FuelVisionSetup.md b/docs/FuelVisionSetup.md new file mode 100644 index 00000000..c2c4b612 --- /dev/null +++ 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 index 98b39c61..ffaf2404 100644 --- 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 index ad01c692..977bc351 100644 --- 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 new file mode 100644 index 00000000..81e0420c --- /dev/null +++ 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 getRobotPose; + + private final List fuelMap = new ArrayList<>(); + + private record FuelEntry(Translation2d position, double timestampSecs) {} + + public FuelVision(Supplier 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 getFuelMap() { + return fuelMap.stream().map(FuelEntry::position).toList(); + } + + /** Nearest fuel to the given pose; empty if map has no entries. */ + public Optional 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 new file mode 100644 index 00000000..c233d8a9 --- /dev/null +++ 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; +}