From f23b4f703d71b77156f6119f7155715783d10e11 Mon Sep 17 00:00:00 2001 From: Sean Monaghan Date: Mon, 9 Feb 2026 17:31:57 -0500 Subject: [PATCH 1/3] initial deployment guide --- DEPLOYMENT.md | 673 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 673 insertions(+) create mode 100644 DEPLOYMENT.md diff --git a/DEPLOYMENT.md b/DEPLOYMENT.md new file mode 100644 index 0000000..ccf2b97 --- /dev/null +++ b/DEPLOYMENT.md @@ -0,0 +1,673 @@ +# Deployment Guide: Test Bot & PhotonVision Camera Calibration + +**Team 2534 BL Lakers | 2026 Season** + +This guide walks through deploying the codebase to the test bot for the first time, with a focus on positioning and calibrating PhotonVision cameras. + +--- + +## Table of Contents + +- [Prerequisites](#prerequisites) +- [Network & Hardware Setup](#network--hardware-setup) +- [Building & Deploying](#building--deploying) +- [First Boot Checklist](#first-boot-checklist) +- [PhotonVision Camera Setup](#photonvision-camera-setup) +- [Camera Calibration Workflow](#camera-calibration-workflow) +- [Updating Camera Transforms in Code](#updating-camera-transforms-in-code) +- [Validating Vision Pose Estimation](#validating-vision-pose-estimation) +- [Monitoring & Telemetry](#monitoring--telemetry) +- [Troubleshooting](#troubleshooting) +- [Code Changes Checklist](#code-changes-checklist) + +--- + +## Prerequisites + +Before arriving at the test bot: + +### On Your Laptop + +| Tool | Purpose | Install | +|------|---------|---------| +| **WPILib 2026** | Build toolchain + Driver Station | [wpilib.org](https://docs.wpilib.org/en/stable/docs/zero-to-robot/step-2/wpilib-setup.html) | +| **FRC Driver Station** | Enable/disable robot | Comes with WPILib (Windows only) | +| **FRC Game Tools** | roboRIO imaging, DS | [NI FRC Game Tools](https://www.ni.com/en/support/downloads/drivers/download.frc-game-tools.html) | +| **PhotonVision Client** | Camera configuration UI | Access via browser at `http://photonvision.local:5800` | +| **Shuffleboard or SmartDashboard** | Live telemetry | Comes with WPILib | +| **AdvantageScope** | Log replay & field visualization | [github.com/Mechanical-Advantage/AdvantageScope](https://github.com/Mechanical-Advantage/AdvantageScope) | +| **JDK 17** | Build the code | Comes with WPILib | + +### On the Robot + +| Component | Details | +|-----------|---------| +| **roboRIO 2** | Imaged with 2026 firmware | +| **REV Power Distribution Hub** | CAN ID 20 | +| **NavX-MXP** | Plugged into MXP SPI port | +| **4x Swerve Modules** | REV SPARK MAX NEOs (CAN IDs: 1-8) | +| **3x PhotonVision Cameras** | USB or Ethernet to coprocessor(s) | +| **PhotonVision Coprocessor(s)** | Raspberry Pi / Orange Pi with PhotonVision installed | +| **Radio** | Programmed for team 2534 | + +### Printed Materials + +- Several **2026 FRC AprilTags** (36h11 family, printed at correct size - 6.5" for 2026) +- A **tape measure** and **angle finder/protractor** for measuring camera positions +- **Masking tape** to mark positions on the floor + +--- + +## Network & Hardware Setup + +### Robot Network + +When connected to the robot's radio, the network follows the FRC convention: + +| Device | Hostname / IP | +|--------|--------------| +| roboRIO | `roborio-2534-frc.local` or `10.25.34.2` | +| Driver Station laptop | `10.25.34.5` (or DHCP) | +| PhotonVision coprocessor(s) | `10.25.34.11`, `.12`, `.13` (static recommended) | + +### Connecting Your Laptop + +1. Connect to the robot's radio WiFi or use a direct Ethernet cable to the radio +2. Verify connectivity: `ping roborio-2534-frc.local` +3. If using mDNS fails, use the IP directly: `ping 10.25.34.2` + +### CAN Bus Wiring Reference + +The deployed code expects these CAN IDs. Verify before deploying: + +``` +Swerve Drivetrain (SPARK MAX / NEO): + Front Left: Drive=3, Steer=4 + Front Right: Drive=6, Steer=5 + Rear Left: Drive=2, Steer=1 + Rear Right: Drive=7, Steer=8 + +Turn Encoders (CTRE Mag Encoder, PWM on DIO): + DIO 0: Rear Left + DIO 1: Front Left + DIO 2: Front Right + DIO 3: Rear Right + +Other: + PDH: CAN 20 + Feeder Motor: CAN 13 + Intake/Launcher Motor: CAN 14 +``` + +> **TEST BOT WARNING:** If your test bot has different CAN IDs than above, you will need to update `Constants.java` before deploying. See [Code Changes Checklist](#code-changes-checklist). + +--- + +## Building & Deploying + +### Step 1: Build Locally First + +From the project root (`2026_Official/`): + +```bash +# Windows +.\gradlew.bat build + +# macOS/Linux +./gradlew build +``` + +This compiles the code, runs tests, and checks formatting. Fix any errors before deploying. + +### Step 2: Deploy to the roboRIO + +Make sure your laptop is on the robot's network, then: + +```bash +# Windows +.\gradlew.bat deploy + +# macOS/Linux +./gradlew deploy +``` + +**What happens:** +1. Code compiles into a fat JAR +2. Static files from `src/main/deploy/` are copied to `/home/lvuser/deploy/` on the roboRIO +3. The JAR is uploaded to the roboRIO +4. Robot code restarts automatically + +### Step 3: Verify Deployment + +1. Open **FRC Driver Station** on your laptop +2. The **Communications** light should go green (connected to roboRIO) +3. The **Robot Code** light should go green (code is running) +4. The robot should be in **Disabled** mode + +If the Robot Code light stays red: +- Check Driver Station console (bottom pane) for Java exceptions +- SSH into the roboRIO and check logs: `ssh admin@10.25.34.2` then `cat /home/lvuser/FRC_UserProgram.log` + +--- + +## First Boot Checklist + +With the robot on blocks (wheels off the ground) and code deployed: + +### 1. Driver Station Verification + +- [ ] Communications light is green +- [ ] Robot Code light is green +- [ ] No errors in the DS console + +### 2. Open Shuffleboard / SmartDashboard + +Connect to NetworkTables at `roborio-2534-frc.local` or `10.25.34.2`. + +- [ ] `Telemetry/Level` shows `LAB` +- [ ] `Telemetry/Initialized` shows `true` +- [ ] `Code Version` appears + +### 3. Gamepad Setup (Driver Station) + +Plug in controllers and assign: + +``` +USB 0: Driver Controller (swerve drive) +USB 1: Manipulator Controller (fuel subsystem) +USB 2: Debug Controller (reserved) +``` + +### 4. Basic Drive Test (on blocks) + +1. Enable **TeleOp** +2. Push driver controller left stick forward - wheels should spin forward +3. Push right stick - robot should attempt to rotate +4. Press **B** on driver controller to reset the gyro +5. Press **right stick button** to toggle wheel lock (X-pattern) + +### 5. Swerve Module Alignment Check + +If wheels point the wrong direction at startup: +- The turn encoder offsets need recalibration for the test bot +- See [Code Changes Checklist](#code-changes-checklist) for how to measure new offsets + +### 6. Vision System Status + +Before cameras are set up, you should see: +- `Vision/Status` = `"No Cameras Connected"` +- `Vision/FrontRightCamera/Connected` = `false` +- `Vision/FrontLeftCamera/Connected` = `false` +- `Vision/RearCamera/Connected` = `false` + +This is expected. Continue to the next section to set up the cameras. + +--- + +## PhotonVision Camera Setup + +### Physical Mounting + +The code expects **3 cameras** mounted as follows (these are defaults - you will measure and update the real values): + +| Camera | Default Position | Default Angle | +|--------|-----------------|---------------| +| **Front-Right** | 30cm forward, 25cm right, 25cm up | 15 deg down, 30 deg right | +| **Front-Left** | 30cm forward, 25cm left, 25cm up | 15 deg down, 30 deg left | +| **Rear** | 30cm backward, centered, 25cm up | 15 deg down, facing backward | + +> **For initial testing**, even **one camera** is enough. You can start with just the front-right camera and add the others later. + +### Coprocessor Setup + +Each camera connects to a coprocessor running PhotonVision. For each coprocessor: + +1. **Flash PhotonVision** onto the coprocessor if not already done + - Download from [photonvision.org](https://photonvision.org) + - Follow the official flashing guide for your hardware (Raspberry Pi, Orange Pi, etc.) + +2. **Connect the camera** via USB to the coprocessor + +3. **Connect the coprocessor** to the robot network (Ethernet to the radio/switch) + +4. **Set a static IP** (recommended): + - Access PhotonVision UI at `http://photonvision.local:5800` + - Go to **Settings** > **Networking** + - Set a static IP in the `10.25.34.x` range: + - Front-Right coprocessor: `10.25.34.11` + - Front-Left coprocessor: `10.25.34.12` + - Rear coprocessor: `10.25.34.13` + - Set the **NetworkTables server** to `10.25.34.2` (roboRIO IP) + +### PhotonVision Pipeline Configuration + +For each camera in the PhotonVision UI: + +1. **Name the camera** (Settings > Camera tab) to match the code: + - `photonvision-front-right` + - `photonvision-front-left` + - `photonvision-rear` + + **CRITICAL:** These names must match **exactly** (case-sensitive). If they don't match, the robot code won't find the camera in NetworkTables. + +2. **Create an AprilTag Pipeline:** + - Click **+ New Pipeline** + - Pipeline type: **AprilTag** + - Tag family: **36h11** + +3. **Enable multi-target mode:** + - In the pipeline settings, enable **Multi-Target** (sometimes called 3D or SolvePNP) + - This allows the coprocessor to compute pose from multiple tags simultaneously + +4. **Tune exposure and brightness:** + - Adjust until AprilTags are reliably detected + - Lower exposure = sharper tags but darker image + - Higher exposure = brighter but more motion blur + - Aim for consistent detection at 1-4 meter range + +5. **Camera calibration (intrinsics):** + - Go to **Cameras** > **Camera Calibration** + - Use a checkerboard pattern (print one from the PhotonVision docs) + - Take 12-15 images from different angles + - Run calibration - this computes lens distortion correction + - **This is essential** for accurate pose estimation + +### Verify Camera Connection from Robot Code + +After setting up at least one camera: + +1. Deploy robot code (if not already): `.\gradlew.bat deploy` +2. Open Shuffleboard +3. Check: + - `Vision/FrontRightCamera/Connected` should be `true` + - `Vision/Status` should change from "No Cameras Connected" to something else + +If a camera shows `false`: +- Verify the camera name matches exactly in PhotonVision UI +- Check that the coprocessor can reach the roboRIO (ping test) +- Restart PhotonVision service on the coprocessor +- Check the DS console for NetworkTables connection errors + +--- + +## Camera Calibration Workflow + +This is the primary goal of this deployment: physically position cameras and measure their transforms so the code can accurately estimate the robot's position on the field. + +### What You Need + +- Tape measure (metric, in meters/centimeters) +- Angle finder or digital inclinometer (for pitch angle) +- Protractor or angle measurement tool (for yaw angle) +- Notepad to record measurements +- Several printed AprilTags placed at known positions + +### Step 1: Mount Cameras and Measure Transforms + +For **each camera**, measure these values from the **robot center** (center of rotation, at floor level): + +1. **X (forward/backward):** Distance forward from robot center in meters + - Positive = forward of center + - Negative = behind center + +2. **Y (left/right):** Distance left/right from robot center in meters + - Positive = left of center + - Negative = right of center + +3. **Z (height):** Distance above the floor in meters + - Always positive + +4. **Pitch (tilt):** Camera tilt angle in degrees + - 0 = looking straight ahead + - Negative = tilted down (most cameras should be tilted 10-20 deg down) + +5. **Yaw (rotation):** Camera horizontal angle in degrees + - 0 = pointing straight forward + - Positive = angled left + - Negative = angled right + - 180 = pointing straight backward + +Record your measurements: + +``` +Front-Right Camera: + X = ______ m Y = ______ m Z = ______ m + Pitch = ______ deg Yaw = ______ deg + +Front-Left Camera: + X = ______ m Y = ______ m Z = ______ m + Pitch = ______ deg Yaw = ______ deg + +Rear Camera: + X = ______ m Y = ______ m Z = ______ m + Pitch = ______ deg Yaw = ______ deg +``` + +### Step 2: Place AprilTags at Known Positions + +For calibration validation, set up a mini test field: + +1. Pick a clear area of floor (at least 4m x 4m) +2. Mark an **origin point** with tape +3. Place 2-3 printed AprilTags at **measured distances** from the origin +4. Use tag IDs that exist in the 2026 field layout (e.g., tags 1-28) +5. Mount tags vertically (facing the robot) at roughly 1.1m height (hub tag height) +6. Record exact positions of each tag + +### Step 3: Test Detection at Various Distances + +With the robot enabled in TeleOp and cameras connected: + +1. Place robot at a known distance from a tag (start at 1 meter) +2. Check Shuffleboard: + - `Vision/{Camera}/TargetCount` > 0 + - `Vision/{Camera}/DetectedTags` shows the correct tag ID + - `Vision/{Camera}/BestTargetYaw` changes as you move the robot laterally +3. Move to 2m, 3m, 4m and note when detection becomes unreliable +4. Record the **maximum reliable detection distance** for each camera + +### Step 4: Validate Pose Estimation + +The vision system already feeds pose estimates into the drivetrain's SwerveDrivePoseEstimator. To validate: + +1. Place the robot at a known position relative to visible AprilTags +2. Open Shuffleboard and watch: + - `Vision/{Camera}/EstimateStatus` should show "Accepted" + - `Vision/{Camera}/EstimateX` and `EstimateY` should roughly match the known position +3. If estimates show "Rejected": + - Single tags beyond 4m are rejected (distance filter) + - High ambiguity tags are rejected (> 0.2 threshold) + - This is working as intended +4. Drive the robot slowly and confirm the pose estimate tracks smoothly +5. Compare `CurrentPoseEstimator` (vision-fused) vs `CurrentPose` (odometry-only) in Shuffleboard + +### Step 5: Iterate on Camera Positions + +Based on testing: +- Adjust camera tilt (pitch) if tags aren't detected at desired ranges +- Adjust camera yaw if the field of view doesn't cover the right area +- Ensure no part of the robot frame blocks the camera view +- After each physical adjustment, **re-measure and update the transforms** + +--- + +## Updating Camera Transforms in Code + +Once you have final measurements, update `VisionSubsystemContext.java`: + +### File: `src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java` + +The default transforms are at lines 54-77. Update them with your measured values: + +```java +// Example: Front-Right camera measured at +// 28cm forward, 22cm right, 30cm up, tilted 12deg down, angled 25deg right +@Builder.Default +private final Transform3d frontRightCameraToRobot = new Transform3d( + new Translation3d(0.28, -0.22, 0.30), + new Rotation3d(0, Math.toRadians(-12), Math.toRadians(-25))); + +// Example: Front-Left camera measured at +// 28cm forward, 22cm left, 30cm up, tilted 12deg down, angled 25deg left +@Builder.Default +private final Transform3d frontLeftCameraToRobot = new Transform3d( + new Translation3d(0.28, 0.22, 0.30), + new Rotation3d(0, Math.toRadians(-12), Math.toRadians(25))); + +// Example: Rear camera measured at +// 25cm backward, centered, 28cm up, tilted 10deg down, facing backward +@Builder.Default +private final Transform3d rearCameraToRobot = new Transform3d( + new Translation3d(-0.25, 0.0, 0.28), + new Rotation3d(0, Math.toRadians(-10), Math.PI)); +``` + +After updating, redeploy: `.\gradlew.bat deploy` + +--- + +## Validating Vision Pose Estimation + +### Using AdvantageScope + +AdvantageScope gives you the best visualization of whether your camera transforms are correct. + +1. **Plug a USB stick** (FAT32 formatted) into the roboRIO USB port +2. Run the robot for a few minutes, driving near AprilTags +3. Retrieve the USB stick - logs are in `FRC_LOGS/` folder +4. Open the `.wpilog` file in AdvantageScope + +In AdvantageScope: +- Add `Drivetrain/Pose` to the **Field 2D** view (odometry-only pose) +- Add `CurrentPoseEstimator` (vision-fused pose) +- If the vision-fused pose jumps wildly, the camera transforms may be wrong +- If the vision-fused pose is smooth and corrects drift, the calibration is good + +### Using Shuffleboard Live + +Key values to monitor during a calibration run: + +| NetworkTables Key | What to Look For | +|-------------------|------------------| +| `Vision/Status` | "Tracking (FR+FL+Rear)" when all cameras see tags | +| `Vision/TotalTagsDetected` | Number of tags currently visible | +| `Vision/FrontRightCamera/EstimateStatus` | "Accepted" means pose was used | +| `Vision/StdDev/XY` | Lower = more trusted. Multi-tag should be ~0.5-1.0 | +| `Vision/TagCount` | 2+ tags = multi-tag mode (more accurate) | +| `Vision/AvgDistance` | Distance to detected tags in meters | +| `CurrentPoseEstimator` | Robot pose with vision corrections applied | + +### Signs of Good Calibration + +- Vision-fused pose (`CurrentPoseEstimator`) is stable and doesn't jump +- Pose corrections are small and smooth when tags become visible +- Multi-tag estimates (2+ tags) produce especially stable poses +- Robot pose on the AdvantageScope field matches physical position + +### Signs of Bad Calibration + +- Pose jumps wildly when vision estimates are accepted +- Robot appears to be in the wrong location on the field visualization +- `EstimateStatus` frequently shows "Rejected" +- Large discrepancy between odometry-only and vision-fused poses + +--- + +## Monitoring & Telemetry + +### Telemetry Level + +The deployed code defaults to `LAB` level, which captures detailed data useful for testing. This is configured in `src/main/deploy/telemetry.properties`: + +```properties +telemetry.level=LAB +``` + +You can change the level live via Shuffleboard by editing `Telemetry/Level`. + +### Key Telemetry to Watch + +#### Drivetrain Health +- `Drivetrain/Pose` - Odometry-only robot position +- `CurrentPoseEstimator` - Vision-fused position +- `Drivetrain/Heading` - Current gyro heading + +#### Vision Health +- `Vision/Status` - Quick overview of camera status +- `Vision/{Camera}/Connected` - Per-camera connection status +- `Vision/{Camera}/TargetCount` - How many tags each camera sees +- `Vision/{Camera}/DetectedTags` - Which tag IDs are visible + +#### Turret Tracker (for hub targeting) +- `TurretTracker/AngleDeg` - Computed aim angle +- `TurretTracker/DistanceM` - Distance to target hub +- `TurretTracker/Mode` - SHOOTING or PASSING mode + +### Log Collection + +For post-session analysis: + +1. **USB stick** (recommended): Insert FAT32 USB into roboRIO before testing + - Logs auto-save to `FRC_LOGS/` on the stick + - File format: `FRC_YYYYMMDD_HHMMSS.wpilog` +2. **Internal storage** (fallback): Logs go to `/home/lvuser/logs/` + - SSH in to retrieve: `scp admin@10.25.34.2:/home/lvuser/logs/*.wpilog .` + +Open logs in **AdvantageScope** for full replay with field visualization. + +--- + +## Troubleshooting + +### Robot Code Won't Start + +| Symptom | Likely Cause | Fix | +|---------|-------------|-----| +| Robot Code light stays red | Java exception on startup | Check DS console for stack trace | +| CAN errors in DS console | Wrong CAN IDs for test bot | Update `Constants.java` Port class | +| "HAL: Resource already allocated" | DIO channel conflict | Check encoder DIO assignments | + +### Cameras Not Detected + +| Symptom | Likely Cause | Fix | +|---------|-------------|-----| +| `Connected = false` for all cameras | Coprocessor not on network | Check Ethernet, ping coprocessor IP | +| `Connected = false` for one camera | Camera name mismatch | Check PhotonVision UI > camera name matches exactly | +| Camera connected but no targets | Pipeline not active | Check PhotonVision UI > pipeline is running (green) | +| Intermittent detection | Poor lighting or focus | Adjust exposure in PhotonVision UI | + +### Swerve Drive Issues + +| Symptom | Likely Cause | Fix | +|---------|-------------|-----| +| Wheels point wrong direction | Turn encoder offsets wrong for test bot | Re-measure offsets (see code changes) | +| Robot drives sideways | Module assignment mismatch | Verify CAN IDs match physical positions | +| Robot spins in circles | Gyro not calibrated | Wait 10 sec after boot, press B to reset | +| Very slow or no movement | Right trigger not pressed | Right trigger = gas pedal | + +### Vision Pose Estimation Issues + +| Symptom | Likely Cause | Fix | +|---------|-------------|-----| +| Pose jumps wildly | Camera transform is wrong | Re-measure physical camera positions | +| All estimates rejected | Tags too far or too ambiguous | Move closer, add more tags | +| Estimates only from one camera | Other cameras misconfigured | Check each camera individually | + +--- + +## Code Changes Checklist + +These are changes you **may** need to make before or during the test bot deployment. + +### Required If Test Bot Has Different CAN IDs + +Update motor CAN IDs in `Constants.java` (`Port` class, lines 107-130): + +```java +public static class Port { + public static final int REAR_LEFT_TURN_CHANNEL = 1; // Update if different + public static final int REAR_LEFT_DRIVE_CHANNEL = 2; // Update if different + public static final int FRONT_LEFT_DRIVE_CHANNEL = 3; // Update if different + // ... etc +} +``` + +### Required: Measure Turn Encoder Offsets for Test Bot + +The current offsets are for the 2025 robot. The test bot will have different values. + +**How to measure offsets:** + +1. Deploy the code with all offsets set to `0`: + ```java + // In Constants.java, temporarily: + public class RobotVersion2025 extends RobotVersionConstants { + public static final double flTurnEncoderOffset = 0; + public static final double frTurnEncoderOffset = 0; + public static final double rlTurnEncoderOffset = 0; + public static final double rrTurnEncoderOffset = 0; + } + ``` +2. Deploy and enable the robot +3. Physically straighten all wheels to point forward (use a straight edge) +4. Read the raw encoder values from Shuffleboard for each module +5. The offset = the raw value when wheels are straight forward +6. Update the constants and redeploy + +### Required: Update Camera Transforms After Measurement + +As described in [Updating Camera Transforms in Code](#updating-camera-transforms-in-code), update `VisionSubsystemContext.java` with measured camera positions. + +### Optional: Starting With Fewer Than 3 Cameras + +If you only have 1 or 2 cameras to start with, the code handles this gracefully - unconnected cameras simply show `Connected = false` and no data. The pose estimator only uses cameras that provide valid data. + +No code changes needed - just physically connect however many cameras you have. + +### Optional: Adjusting Vision Tuning Parameters + +If vision estimates are too noisy or too conservative, adjust in `VisionSubsystemContext.java`: + +```java +// Reject estimates from tags further than this (meters) +private final double maxPoseEstimationDistance = 4.0; // Increase for larger test area + +// Reject single-tag estimates with ambiguity above this +private final double poseAmbiguityThreshold = 0.2; // Increase to accept more, decrease for stricter + +// Trust level: lower = more trusted +private final double singleTagStdDevFactor = 4.0; // Single tag trust (higher = less trust) +private final double multiTagStdDevFactor = 0.5; // Multi tag trust (lower = more trust) +``` + +### Optional: Telemetry Level for Verbose Debugging + +If you need maximum diagnostic data during calibration, edit `src/main/deploy/telemetry.properties`: + +```properties +telemetry.level=VERBOSE +``` + +This logs per-tag yaw/pitch/area/skew data for every detected AprilTag. Useful during calibration, but generates large log files. + +--- + +## Quick Reference: Deploy Cycle + +``` +1. Make code changes +2. Build: .\gradlew.bat build +3. Deploy: .\gradlew.bat deploy +4. Open Driver Station +5. Open Shuffleboard (connect to 10.25.34.2) +6. Enable robot (TeleOp for driving, Test for diagnostics) +7. Monitor telemetry +8. Disable robot +9. Retrieve USB logs if needed +10. Repeat +``` + +## Quick Reference: Camera Names + +| Code Name | PhotonVision NetworkTables Name | +|-----------|-------------------------------| +| Front-Right Camera | `photonvision-front-right` | +| Front-Left Camera | `photonvision-front-left` | +| Rear Camera | `photonvision-rear` | + +## Quick Reference: Controller Layout + +``` +DRIVER (USB 0): + Left Stick = Drive (translation) + Right Stick = Rotate + Right Trigger = Gas (acceleration) + Left Trigger = Half-speed (hold > 50%) + B Button = Reset gyro + R-Stick Click = Wheel lock toggle + +MANIPULATOR (USB 1): + Left Bumper = Fuel intake (hold) + Right Bumper = Fuel launch (hold: 1s spinup then launch) + X Button = Fuel eject (hold) +``` From 7a5ab71d4b8204b278ea8f9aba0bd14b6bf053ad Mon Sep 17 00:00:00 2001 From: Sean Monaghan Date: Mon, 9 Feb 2026 18:06:52 -0500 Subject: [PATCH 2/3] initial quad cam --- .../subsystems/vision/VisionSubsystem.java | 170 ++++++++++++------ .../vision/VisionSubsystemContext.java | 36 ++-- 2 files changed, 144 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index e3e43e8..4e932a8 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -36,11 +36,11 @@ /** * Vision subsystem for AprilTag detection using PhotonVision. - * Manages three cameras (front-right, front-left, rear), provides pose estimation, - * supports simulation, and visualizes camera FOV cones. + * Manages four cameras (front-right, front-left, right-side, left-side), + * provides pose estimation, supports simulation, and visualizes camera FOV cones. * * Features: - * - Triple camera support (front-right, front-left, rear) + * - Quad camera support (front-right, front-left, right-side, left-side) * - PhotonPoseEstimator integration for robot localization * - Dynamic standard deviation calculation * - Full simulation support with VisionSystemSim @@ -63,24 +63,28 @@ public interface VisionMeasurementConsumer { private final VisionMeasurementConsumer visionMeasurementConsumer; private final PhotonCamera frontRightCamera; private final PhotonCamera frontLeftCamera; - private final PhotonCamera rearCamera; + private final PhotonCamera rightSideCamera; + private final PhotonCamera leftSideCamera; // Pose estimation private final AprilTagFieldLayout fieldLayout; private final PhotonPoseEstimator frontRightPoseEstimator; private final PhotonPoseEstimator frontLeftPoseEstimator; - private final PhotonPoseEstimator rearPoseEstimator; + private final PhotonPoseEstimator rightSidePoseEstimator; + private final PhotonPoseEstimator leftSidePoseEstimator; // Simulation (only created in simulation mode) private VisionSystemSim visionSim; private PhotonCameraSim frontRightCameraSim; private PhotonCameraSim frontLeftCameraSim; - private PhotonCameraSim rearCameraSim; + private PhotonCameraSim rightSideCameraSim; + private PhotonCameraSim leftSideCameraSim; // FOV visualization publishers (simulation only) private StructArrayPublisher frontRightFovPublisher; private StructArrayPublisher frontLeftFovPublisher; - private StructArrayPublisher rearFovPublisher; + private StructArrayPublisher rightSideFovPublisher; + private StructArrayPublisher leftSideFovPublisher; private Mechanism2d cameraLayoutMech; /** @@ -102,7 +106,8 @@ public VisionSubsystem( // Initialize PhotonVision cameras this.frontRightCamera = new PhotonCamera(context.getFrontRightCameraName()); this.frontLeftCamera = new PhotonCamera(context.getFrontLeftCameraName()); - this.rearCamera = new PhotonCamera(context.getRearCameraName()); + this.rightSideCamera = new PhotonCamera(context.getRightSideCameraName()); + this.leftSideCamera = new PhotonCamera(context.getLeftSideCameraName()); // Load AprilTag field layout from WPILib this.fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); @@ -112,8 +117,10 @@ public VisionSubsystem( fieldLayout, context.getPoseEstimationStrategy(), context.getFrontRightCameraToRobot()); this.frontLeftPoseEstimator = new PhotonPoseEstimator( fieldLayout, context.getPoseEstimationStrategy(), context.getFrontLeftCameraToRobot()); - this.rearPoseEstimator = new PhotonPoseEstimator( - fieldLayout, context.getPoseEstimationStrategy(), context.getRearCameraToRobot()); + this.rightSidePoseEstimator = new PhotonPoseEstimator( + fieldLayout, context.getPoseEstimationStrategy(), context.getRightSideCameraToRobot()); + this.leftSidePoseEstimator = new PhotonPoseEstimator( + fieldLayout, context.getPoseEstimationStrategy(), context.getLeftSideCameraToRobot()); // Initialize simulation if enabled // NOTE: PhotonVision simulation is expensive (~96ms per loop) and causes "CommandScheduler @@ -132,7 +139,8 @@ public VisionSubsystem( Telemetry.publish("Vision/Status", "Initialized", TelemetryLevel.MATCH); Telemetry.publish("Vision/FrontRightCamera/Connected", false, TelemetryLevel.MATCH); Telemetry.publish("Vision/FrontLeftCamera/Connected", false, TelemetryLevel.MATCH); - Telemetry.publish("Vision/RearCamera/Connected", false, TelemetryLevel.MATCH); + Telemetry.publish("Vision/RightSideCamera/Connected", false, TelemetryLevel.MATCH); + Telemetry.publish("Vision/LeftSideCamera/Connected", false, TelemetryLevel.MATCH); } /** @@ -160,16 +168,25 @@ private void initializeSimulation() { frontLeftCameraSim.enableRawStream(false); frontLeftCameraSim.enableProcessedStream(false); - // Configure rear camera simulation - SimCameraProperties rearProps = createSimCameraProperties(); - rearCameraSim = new PhotonCameraSim(rearCamera, rearProps); - visionSim.addCamera(rearCameraSim, context.getRearCameraToRobot()); - rearCameraSim.enableDrawWireframe(true); + // Configure right-side camera simulation + SimCameraProperties rightSideProps = createSimCameraProperties(); + rightSideCameraSim = new PhotonCameraSim(rightSideCamera, rightSideProps); + visionSim.addCamera(rightSideCameraSim, context.getRightSideCameraToRobot()); + rightSideCameraSim.enableDrawWireframe(true); // Disable video streaming to avoid CameraServer handle issues - rearCameraSim.enableRawStream(false); - rearCameraSim.enableProcessedStream(false); + rightSideCameraSim.enableRawStream(false); + rightSideCameraSim.enableProcessedStream(false); + + // Configure left-side camera simulation + SimCameraProperties leftSideProps = createSimCameraProperties(); + leftSideCameraSim = new PhotonCameraSim(leftSideCamera, leftSideProps); + visionSim.addCamera(leftSideCameraSim, context.getLeftSideCameraToRobot()); + leftSideCameraSim.enableDrawWireframe(true); + // Disable video streaming to avoid CameraServer handle issues + leftSideCameraSim.enableRawStream(false); + leftSideCameraSim.enableProcessedStream(false); - Telemetry.publish("Vision/Simulation", "Active (3 cameras)", TelemetryLevel.LAB); + Telemetry.publish("Vision/Simulation", "Active (4 cameras)", TelemetryLevel.LAB); } /** @@ -201,10 +218,12 @@ private void initializeFovVisualization() { .publish(); frontLeftFovPublisher = nti.getStructArrayTopic("Vision/FrontLeft/FOVCone", Pose2d.struct) .publish(); - rearFovPublisher = - nti.getStructArrayTopic("Vision/Rear/FOVCone", Pose2d.struct).publish(); + rightSideFovPublisher = nti.getStructArrayTopic("Vision/RightSide/FOVCone", Pose2d.struct) + .publish(); + leftSideFovPublisher = nti.getStructArrayTopic("Vision/LeftSide/FOVCone", Pose2d.struct) + .publish(); - // Mechanism2d: top-down camera layout (robot center, 3 directional lines) + // Mechanism2d: top-down camera layout (robot center, 4 directional lines) double mechSize = 100.0; cameraLayoutMech = new Mechanism2d(mechSize, mechSize); MechanismRoot2d center = cameraLayoutMech.getRoot("robotCenter", mechSize / 2.0, mechSize / 2.0); @@ -214,8 +233,10 @@ private void initializeFovVisualization() { center.append(new MechanismLigament2d("frontRightCam", 30, 90 - 30, 2, new Color8Bit(Color.kOrange))); // Front-left at yaw=+30deg: mechanism angle = 90 + 30 = 120 center.append(new MechanismLigament2d("frontLeftCam", 30, 90 + 30, 2, new Color8Bit(Color.kYellow))); - // Rear at yaw=180deg: mechanism angle = 90 + 180 = 270 - center.append(new MechanismLigament2d("rearCam", 30, 270, 2, new Color8Bit(Color.kCyan))); + // Right-side at yaw=-90deg: mechanism angle = 90 + (-90) = 0 + center.append(new MechanismLigament2d("rightSideCam", 30, 0, 2, new Color8Bit(Color.kCyan))); + // Left-side at yaw=+90deg: mechanism angle = 90 + 90 = 180 + center.append(new MechanismLigament2d("leftSideCam", 30, 180, 2, new Color8Bit(Color.kMagenta))); Telemetry.putData("Vision/CameraLayout", cameraLayoutMech); } @@ -227,11 +248,13 @@ private void updatePoseEstimation() { Pose2d currentPose = drivetrain.getPose2dEstimator(); frontRightPoseEstimator.setReferencePose(currentPose); frontLeftPoseEstimator.setReferencePose(currentPose); - rearPoseEstimator.setReferencePose(currentPose); + rightSidePoseEstimator.setReferencePose(currentPose); + leftSidePoseEstimator.setReferencePose(currentPose); processCamera(frontRightCamera, frontRightPoseEstimator, "FrontRight"); processCamera(frontLeftCamera, frontLeftPoseEstimator, "FrontLeft"); - processCamera(rearCamera, rearPoseEstimator, "Rear"); + processCamera(rightSideCamera, rightSidePoseEstimator, "RightSide"); + processCamera(leftSideCamera, leftSidePoseEstimator, "LeftSide"); } /** @@ -331,15 +354,18 @@ public void periodic() { boolean frontRightConnected = isSimulation || frontRightCamera.isConnected(); boolean frontLeftConnected = isSimulation || frontLeftCamera.isConnected(); - boolean rearConnected = isSimulation || rearCamera.isConnected(); + boolean rightSideConnected = isSimulation || rightSideCamera.isConnected(); + boolean leftSideConnected = isSimulation || leftSideCamera.isConnected(); Telemetry.publish("Vision/FrontRightCamera/Connected", frontRightConnected, TelemetryLevel.MATCH); Telemetry.publish("Vision/FrontLeftCamera/Connected", frontLeftConnected, TelemetryLevel.MATCH); - Telemetry.publish("Vision/RearCamera/Connected", rearConnected, TelemetryLevel.MATCH); + Telemetry.publish("Vision/RightSideCamera/Connected", rightSideConnected, TelemetryLevel.MATCH); + Telemetry.publish("Vision/LeftSideCamera/Connected", leftSideConnected, TelemetryLevel.MATCH); PhotonPipelineResult frontRightResult = frontRightCamera.getLatestResult(); PhotonPipelineResult frontLeftResult = frontLeftCamera.getLatestResult(); - PhotonPipelineResult rearResult = rearCamera.getLatestResult(); + PhotonPipelineResult rightSideResult = rightSideCamera.getLatestResult(); + PhotonPipelineResult leftSideResult = leftSideCamera.getLatestResult(); if (frontRightConnected && frontRightResult.hasTargets()) { processAndLogTargets("FrontRight", frontRightResult); @@ -355,15 +381,29 @@ public void periodic() { Telemetry.publish("Vision/FrontLeftCamera/DetectedTags", "None", TelemetryLevel.LAB); } - if (rearConnected && rearResult.hasTargets()) { - processAndLogTargets("Rear", rearResult); + if (rightSideConnected && rightSideResult.hasTargets()) { + processAndLogTargets("RightSide", rightSideResult); + } else { + Telemetry.publish("Vision/RightSideCamera/TargetCount", 0, TelemetryLevel.MATCH); + Telemetry.publish("Vision/RightSideCamera/DetectedTags", "None", TelemetryLevel.LAB); + } + + if (leftSideConnected && leftSideResult.hasTargets()) { + processAndLogTargets("LeftSide", leftSideResult); } else { - Telemetry.publish("Vision/RearCamera/TargetCount", 0, TelemetryLevel.MATCH); - Telemetry.publish("Vision/RearCamera/DetectedTags", "None", TelemetryLevel.LAB); + Telemetry.publish("Vision/LeftSideCamera/TargetCount", 0, TelemetryLevel.MATCH); + Telemetry.publish("Vision/LeftSideCamera/DetectedTags", "None", TelemetryLevel.LAB); } updateSystemStatus( - frontRightConnected, frontLeftConnected, rearConnected, frontRightResult, frontLeftResult, rearResult); + frontRightConnected, + frontLeftConnected, + rightSideConnected, + leftSideConnected, + frontRightResult, + frontLeftResult, + rightSideResult, + leftSideResult); updatePoseEstimation(); } @@ -392,7 +432,8 @@ private void updateFovVisualization(Pose2d robotPose) { publishCameraFov( frontRightFovPublisher, robotPose, context.getFrontRightCameraToRobot(), halfFovRad, rayLength); publishCameraFov(frontLeftFovPublisher, robotPose, context.getFrontLeftCameraToRobot(), halfFovRad, rayLength); - publishCameraFov(rearFovPublisher, robotPose, context.getRearCameraToRobot(), halfFovRad, rayLength); + publishCameraFov(rightSideFovPublisher, robotPose, context.getRightSideCameraToRobot(), halfFovRad, rayLength); + publishCameraFov(leftSideFovPublisher, robotPose, context.getLeftSideCameraToRobot(), halfFovRad, rayLength); } /** @@ -473,35 +514,40 @@ private void processAndLogTargets(String cameraName, PhotonPipelineResult result } /** - * Updates overall system status telemetry for 3 cameras. + * Updates overall system status telemetry for 4 cameras. */ private void updateSystemStatus( boolean frontRightConnected, boolean frontLeftConnected, - boolean rearConnected, + boolean rightSideConnected, + boolean leftSideConnected, PhotonPipelineResult frontRightResult, PhotonPipelineResult frontLeftResult, - PhotonPipelineResult rearResult) { + PhotonPipelineResult rightSideResult, + PhotonPipelineResult leftSideResult) { int connectedCount = 0; if (frontRightConnected) connectedCount++; if (frontLeftConnected) connectedCount++; - if (rearConnected) connectedCount++; + if (rightSideConnected) connectedCount++; + if (leftSideConnected) connectedCount++; String status; if (connectedCount == 0) { status = "No Cameras Connected"; - } else if (connectedCount < 3) { + } else if (connectedCount < 4) { List offline = new ArrayList<>(); if (!frontRightConnected) offline.add("FrontRight"); if (!frontLeftConnected) offline.add("FrontLeft"); - if (!rearConnected) offline.add("Rear"); + if (!rightSideConnected) offline.add("RightSide"); + if (!leftSideConnected) offline.add("LeftSide"); status = String.join(", ", offline) + " Offline"; } else { List trackingCams = new ArrayList<>(); if (frontRightResult.hasTargets()) trackingCams.add("FR"); if (frontLeftResult.hasTargets()) trackingCams.add("FL"); - if (rearResult.hasTargets()) trackingCams.add("Rear"); + if (rightSideResult.hasTargets()) trackingCams.add("RS"); + if (leftSideResult.hasTargets()) trackingCams.add("LS"); if (trackingCams.isEmpty()) { status = "No Targets Detected"; @@ -519,8 +565,11 @@ private void updateSystemStatus( if (frontLeftConnected && frontLeftResult.hasTargets()) { totalTags += frontLeftResult.getTargets().size(); } - if (rearConnected && rearResult.hasTargets()) { - totalTags += rearResult.getTargets().size(); + if (rightSideConnected && rightSideResult.hasTargets()) { + totalTags += rightSideResult.getTargets().size(); + } + if (leftSideConnected && leftSideResult.hasTargets()) { + totalTags += leftSideResult.getTargets().size(); } Telemetry.publish("Vision/TotalTagsDetected", totalTags, TelemetryLevel.MATCH); } @@ -535,8 +584,12 @@ public PhotonPipelineResult getFrontLeftCameraResult() { return frontLeftCamera.getLatestResult(); } - public PhotonPipelineResult getRearCameraResult() { - return rearCamera.getLatestResult(); + public PhotonPipelineResult getRightSideCameraResult() { + return rightSideCamera.getLatestResult(); + } + + public PhotonPipelineResult getLeftSideCameraResult() { + return leftSideCamera.getLatestResult(); } public PhotonCamera getFrontRightCamera() { @@ -547,8 +600,12 @@ public PhotonCamera getFrontLeftCamera() { return frontLeftCamera; } - public PhotonCamera getRearCamera() { - return rearCamera; + public PhotonCamera getRightSideCamera() { + return rightSideCamera; + } + + public PhotonCamera getLeftSideCamera() { + return leftSideCamera; } public boolean isFrontRightCameraConnected() { @@ -559,8 +616,12 @@ public boolean isFrontLeftCameraConnected() { return frontLeftCamera.isConnected(); } - public boolean isRearCameraConnected() { - return rearCamera.isConnected(); + public boolean isRightSideCameraConnected() { + return rightSideCamera.isConnected(); + } + + public boolean isLeftSideCameraConnected() { + return leftSideCamera.isConnected(); } public int getFrontRightTargetCount() { @@ -573,8 +634,13 @@ public int getFrontLeftTargetCount() { return result.hasTargets() ? result.getTargets().size() : 0; } - public int getRearTargetCount() { - PhotonPipelineResult result = rearCamera.getLatestResult(); + public int getRightSideTargetCount() { + PhotonPipelineResult result = rightSideCamera.getLatestResult(); + return result.hasTargets() ? result.getTargets().size() : 0; + } + + public int getLeftSideTargetCount() { + PhotonPipelineResult result = leftSideCamera.getLatestResult(); return result.hasTargets() ? result.getTargets().size() : 0; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java index ba49c71..eb6ddb8 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java @@ -9,8 +9,8 @@ /** * Configuration context for the Vision subsystem using PhotonVision. - * Supports three cameras (front-right, front-left, rear) for AprilTag detection - * and localization, with FOV visualization in simulation. + * Supports four cameras (front-right, front-left, right-side, left-side) + * for AprilTag detection and localization, with FOV visualization in simulation. */ @Data @Builder @@ -29,10 +29,16 @@ public class VisionSubsystemContext { private final String frontLeftCameraName = "photonvision-front-left"; /** - * Network table name for the rear camera + * Network table name for the right-side camera */ @Builder.Default - private final String rearCameraName = "photonvision-rear"; + private final String rightSideCameraName = "photonvision-right-side"; + + /** + * Network table name for the left-side camera + */ + @Builder.Default + private final String leftSideCameraName = "photonvision-left-side"; /** * Whether to enable verbose logging to SmartDashboard @@ -67,14 +73,24 @@ public class VisionSubsystemContext { new Translation3d(0.30, 0.25, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(30))); /** - * Transform from robot center to rear camera optical center. - * Mounted centered on the rear of the robot, facing backward. - * Position: X=-0.30m backward, Y=0 centered, Z=+0.25m up. - * Rotation: pitch=-15deg (tilted down), yaw=180deg (facing backward). + * Transform from robot center to right-side camera optical center. + * Mounted on the right side of the robot, facing perpendicular to the right. + * Position: X=0.0m (centered fore-aft), Y=-0.30m right, Z=+0.25m up. + * Rotation: pitch=-15deg (tilted down), yaw=-90deg (facing right). + */ + @Builder.Default + private final Transform3d rightSideCameraToRobot = new Transform3d( + new Translation3d(0.0, -0.30, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-90))); + + /** + * Transform from robot center to left-side camera optical center. + * Mounted on the left side of the robot, facing perpendicular to the left. + * Position: X=0.0m (centered fore-aft), Y=+0.30m left, Z=+0.25m up. + * Rotation: pitch=-15deg (tilted down), yaw=+90deg (facing left). */ @Builder.Default - private final Transform3d rearCameraToRobot = - new Transform3d(new Translation3d(-0.30, 0.0, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.PI)); + private final Transform3d leftSideCameraToRobot = new Transform3d( + new Translation3d(0.0, 0.30, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(90))); /** * Whether to enable simulation features (VisionSystemSim) From 1dbb3a984eded2b2d9b986c1c0e7764403ab3211 Mon Sep 17 00:00:00 2001 From: Sean Monaghan Date: Wed, 11 Feb 2026 09:00:03 -0500 Subject: [PATCH 3/3] account for hub height during turret tracking --- DEPLOYMENT.md | 673 ------------------ .../turrettracker/TurretTracker.java | 81 ++- .../turrettracker/TurretTrackerContext.java | 19 +- .../subsystems/vision/VisionSubsystem.java | 51 +- .../vision/VisionSubsystemContext.java | 24 +- 5 files changed, 121 insertions(+), 727 deletions(-) delete mode 100644 DEPLOYMENT.md diff --git a/DEPLOYMENT.md b/DEPLOYMENT.md deleted file mode 100644 index ccf2b97..0000000 --- a/DEPLOYMENT.md +++ /dev/null @@ -1,673 +0,0 @@ -# Deployment Guide: Test Bot & PhotonVision Camera Calibration - -**Team 2534 BL Lakers | 2026 Season** - -This guide walks through deploying the codebase to the test bot for the first time, with a focus on positioning and calibrating PhotonVision cameras. - ---- - -## Table of Contents - -- [Prerequisites](#prerequisites) -- [Network & Hardware Setup](#network--hardware-setup) -- [Building & Deploying](#building--deploying) -- [First Boot Checklist](#first-boot-checklist) -- [PhotonVision Camera Setup](#photonvision-camera-setup) -- [Camera Calibration Workflow](#camera-calibration-workflow) -- [Updating Camera Transforms in Code](#updating-camera-transforms-in-code) -- [Validating Vision Pose Estimation](#validating-vision-pose-estimation) -- [Monitoring & Telemetry](#monitoring--telemetry) -- [Troubleshooting](#troubleshooting) -- [Code Changes Checklist](#code-changes-checklist) - ---- - -## Prerequisites - -Before arriving at the test bot: - -### On Your Laptop - -| Tool | Purpose | Install | -|------|---------|---------| -| **WPILib 2026** | Build toolchain + Driver Station | [wpilib.org](https://docs.wpilib.org/en/stable/docs/zero-to-robot/step-2/wpilib-setup.html) | -| **FRC Driver Station** | Enable/disable robot | Comes with WPILib (Windows only) | -| **FRC Game Tools** | roboRIO imaging, DS | [NI FRC Game Tools](https://www.ni.com/en/support/downloads/drivers/download.frc-game-tools.html) | -| **PhotonVision Client** | Camera configuration UI | Access via browser at `http://photonvision.local:5800` | -| **Shuffleboard or SmartDashboard** | Live telemetry | Comes with WPILib | -| **AdvantageScope** | Log replay & field visualization | [github.com/Mechanical-Advantage/AdvantageScope](https://github.com/Mechanical-Advantage/AdvantageScope) | -| **JDK 17** | Build the code | Comes with WPILib | - -### On the Robot - -| Component | Details | -|-----------|---------| -| **roboRIO 2** | Imaged with 2026 firmware | -| **REV Power Distribution Hub** | CAN ID 20 | -| **NavX-MXP** | Plugged into MXP SPI port | -| **4x Swerve Modules** | REV SPARK MAX NEOs (CAN IDs: 1-8) | -| **3x PhotonVision Cameras** | USB or Ethernet to coprocessor(s) | -| **PhotonVision Coprocessor(s)** | Raspberry Pi / Orange Pi with PhotonVision installed | -| **Radio** | Programmed for team 2534 | - -### Printed Materials - -- Several **2026 FRC AprilTags** (36h11 family, printed at correct size - 6.5" for 2026) -- A **tape measure** and **angle finder/protractor** for measuring camera positions -- **Masking tape** to mark positions on the floor - ---- - -## Network & Hardware Setup - -### Robot Network - -When connected to the robot's radio, the network follows the FRC convention: - -| Device | Hostname / IP | -|--------|--------------| -| roboRIO | `roborio-2534-frc.local` or `10.25.34.2` | -| Driver Station laptop | `10.25.34.5` (or DHCP) | -| PhotonVision coprocessor(s) | `10.25.34.11`, `.12`, `.13` (static recommended) | - -### Connecting Your Laptop - -1. Connect to the robot's radio WiFi or use a direct Ethernet cable to the radio -2. Verify connectivity: `ping roborio-2534-frc.local` -3. If using mDNS fails, use the IP directly: `ping 10.25.34.2` - -### CAN Bus Wiring Reference - -The deployed code expects these CAN IDs. Verify before deploying: - -``` -Swerve Drivetrain (SPARK MAX / NEO): - Front Left: Drive=3, Steer=4 - Front Right: Drive=6, Steer=5 - Rear Left: Drive=2, Steer=1 - Rear Right: Drive=7, Steer=8 - -Turn Encoders (CTRE Mag Encoder, PWM on DIO): - DIO 0: Rear Left - DIO 1: Front Left - DIO 2: Front Right - DIO 3: Rear Right - -Other: - PDH: CAN 20 - Feeder Motor: CAN 13 - Intake/Launcher Motor: CAN 14 -``` - -> **TEST BOT WARNING:** If your test bot has different CAN IDs than above, you will need to update `Constants.java` before deploying. See [Code Changes Checklist](#code-changes-checklist). - ---- - -## Building & Deploying - -### Step 1: Build Locally First - -From the project root (`2026_Official/`): - -```bash -# Windows -.\gradlew.bat build - -# macOS/Linux -./gradlew build -``` - -This compiles the code, runs tests, and checks formatting. Fix any errors before deploying. - -### Step 2: Deploy to the roboRIO - -Make sure your laptop is on the robot's network, then: - -```bash -# Windows -.\gradlew.bat deploy - -# macOS/Linux -./gradlew deploy -``` - -**What happens:** -1. Code compiles into a fat JAR -2. Static files from `src/main/deploy/` are copied to `/home/lvuser/deploy/` on the roboRIO -3. The JAR is uploaded to the roboRIO -4. Robot code restarts automatically - -### Step 3: Verify Deployment - -1. Open **FRC Driver Station** on your laptop -2. The **Communications** light should go green (connected to roboRIO) -3. The **Robot Code** light should go green (code is running) -4. The robot should be in **Disabled** mode - -If the Robot Code light stays red: -- Check Driver Station console (bottom pane) for Java exceptions -- SSH into the roboRIO and check logs: `ssh admin@10.25.34.2` then `cat /home/lvuser/FRC_UserProgram.log` - ---- - -## First Boot Checklist - -With the robot on blocks (wheels off the ground) and code deployed: - -### 1. Driver Station Verification - -- [ ] Communications light is green -- [ ] Robot Code light is green -- [ ] No errors in the DS console - -### 2. Open Shuffleboard / SmartDashboard - -Connect to NetworkTables at `roborio-2534-frc.local` or `10.25.34.2`. - -- [ ] `Telemetry/Level` shows `LAB` -- [ ] `Telemetry/Initialized` shows `true` -- [ ] `Code Version` appears - -### 3. Gamepad Setup (Driver Station) - -Plug in controllers and assign: - -``` -USB 0: Driver Controller (swerve drive) -USB 1: Manipulator Controller (fuel subsystem) -USB 2: Debug Controller (reserved) -``` - -### 4. Basic Drive Test (on blocks) - -1. Enable **TeleOp** -2. Push driver controller left stick forward - wheels should spin forward -3. Push right stick - robot should attempt to rotate -4. Press **B** on driver controller to reset the gyro -5. Press **right stick button** to toggle wheel lock (X-pattern) - -### 5. Swerve Module Alignment Check - -If wheels point the wrong direction at startup: -- The turn encoder offsets need recalibration for the test bot -- See [Code Changes Checklist](#code-changes-checklist) for how to measure new offsets - -### 6. Vision System Status - -Before cameras are set up, you should see: -- `Vision/Status` = `"No Cameras Connected"` -- `Vision/FrontRightCamera/Connected` = `false` -- `Vision/FrontLeftCamera/Connected` = `false` -- `Vision/RearCamera/Connected` = `false` - -This is expected. Continue to the next section to set up the cameras. - ---- - -## PhotonVision Camera Setup - -### Physical Mounting - -The code expects **3 cameras** mounted as follows (these are defaults - you will measure and update the real values): - -| Camera | Default Position | Default Angle | -|--------|-----------------|---------------| -| **Front-Right** | 30cm forward, 25cm right, 25cm up | 15 deg down, 30 deg right | -| **Front-Left** | 30cm forward, 25cm left, 25cm up | 15 deg down, 30 deg left | -| **Rear** | 30cm backward, centered, 25cm up | 15 deg down, facing backward | - -> **For initial testing**, even **one camera** is enough. You can start with just the front-right camera and add the others later. - -### Coprocessor Setup - -Each camera connects to a coprocessor running PhotonVision. For each coprocessor: - -1. **Flash PhotonVision** onto the coprocessor if not already done - - Download from [photonvision.org](https://photonvision.org) - - Follow the official flashing guide for your hardware (Raspberry Pi, Orange Pi, etc.) - -2. **Connect the camera** via USB to the coprocessor - -3. **Connect the coprocessor** to the robot network (Ethernet to the radio/switch) - -4. **Set a static IP** (recommended): - - Access PhotonVision UI at `http://photonvision.local:5800` - - Go to **Settings** > **Networking** - - Set a static IP in the `10.25.34.x` range: - - Front-Right coprocessor: `10.25.34.11` - - Front-Left coprocessor: `10.25.34.12` - - Rear coprocessor: `10.25.34.13` - - Set the **NetworkTables server** to `10.25.34.2` (roboRIO IP) - -### PhotonVision Pipeline Configuration - -For each camera in the PhotonVision UI: - -1. **Name the camera** (Settings > Camera tab) to match the code: - - `photonvision-front-right` - - `photonvision-front-left` - - `photonvision-rear` - - **CRITICAL:** These names must match **exactly** (case-sensitive). If they don't match, the robot code won't find the camera in NetworkTables. - -2. **Create an AprilTag Pipeline:** - - Click **+ New Pipeline** - - Pipeline type: **AprilTag** - - Tag family: **36h11** - -3. **Enable multi-target mode:** - - In the pipeline settings, enable **Multi-Target** (sometimes called 3D or SolvePNP) - - This allows the coprocessor to compute pose from multiple tags simultaneously - -4. **Tune exposure and brightness:** - - Adjust until AprilTags are reliably detected - - Lower exposure = sharper tags but darker image - - Higher exposure = brighter but more motion blur - - Aim for consistent detection at 1-4 meter range - -5. **Camera calibration (intrinsics):** - - Go to **Cameras** > **Camera Calibration** - - Use a checkerboard pattern (print one from the PhotonVision docs) - - Take 12-15 images from different angles - - Run calibration - this computes lens distortion correction - - **This is essential** for accurate pose estimation - -### Verify Camera Connection from Robot Code - -After setting up at least one camera: - -1. Deploy robot code (if not already): `.\gradlew.bat deploy` -2. Open Shuffleboard -3. Check: - - `Vision/FrontRightCamera/Connected` should be `true` - - `Vision/Status` should change from "No Cameras Connected" to something else - -If a camera shows `false`: -- Verify the camera name matches exactly in PhotonVision UI -- Check that the coprocessor can reach the roboRIO (ping test) -- Restart PhotonVision service on the coprocessor -- Check the DS console for NetworkTables connection errors - ---- - -## Camera Calibration Workflow - -This is the primary goal of this deployment: physically position cameras and measure their transforms so the code can accurately estimate the robot's position on the field. - -### What You Need - -- Tape measure (metric, in meters/centimeters) -- Angle finder or digital inclinometer (for pitch angle) -- Protractor or angle measurement tool (for yaw angle) -- Notepad to record measurements -- Several printed AprilTags placed at known positions - -### Step 1: Mount Cameras and Measure Transforms - -For **each camera**, measure these values from the **robot center** (center of rotation, at floor level): - -1. **X (forward/backward):** Distance forward from robot center in meters - - Positive = forward of center - - Negative = behind center - -2. **Y (left/right):** Distance left/right from robot center in meters - - Positive = left of center - - Negative = right of center - -3. **Z (height):** Distance above the floor in meters - - Always positive - -4. **Pitch (tilt):** Camera tilt angle in degrees - - 0 = looking straight ahead - - Negative = tilted down (most cameras should be tilted 10-20 deg down) - -5. **Yaw (rotation):** Camera horizontal angle in degrees - - 0 = pointing straight forward - - Positive = angled left - - Negative = angled right - - 180 = pointing straight backward - -Record your measurements: - -``` -Front-Right Camera: - X = ______ m Y = ______ m Z = ______ m - Pitch = ______ deg Yaw = ______ deg - -Front-Left Camera: - X = ______ m Y = ______ m Z = ______ m - Pitch = ______ deg Yaw = ______ deg - -Rear Camera: - X = ______ m Y = ______ m Z = ______ m - Pitch = ______ deg Yaw = ______ deg -``` - -### Step 2: Place AprilTags at Known Positions - -For calibration validation, set up a mini test field: - -1. Pick a clear area of floor (at least 4m x 4m) -2. Mark an **origin point** with tape -3. Place 2-3 printed AprilTags at **measured distances** from the origin -4. Use tag IDs that exist in the 2026 field layout (e.g., tags 1-28) -5. Mount tags vertically (facing the robot) at roughly 1.1m height (hub tag height) -6. Record exact positions of each tag - -### Step 3: Test Detection at Various Distances - -With the robot enabled in TeleOp and cameras connected: - -1. Place robot at a known distance from a tag (start at 1 meter) -2. Check Shuffleboard: - - `Vision/{Camera}/TargetCount` > 0 - - `Vision/{Camera}/DetectedTags` shows the correct tag ID - - `Vision/{Camera}/BestTargetYaw` changes as you move the robot laterally -3. Move to 2m, 3m, 4m and note when detection becomes unreliable -4. Record the **maximum reliable detection distance** for each camera - -### Step 4: Validate Pose Estimation - -The vision system already feeds pose estimates into the drivetrain's SwerveDrivePoseEstimator. To validate: - -1. Place the robot at a known position relative to visible AprilTags -2. Open Shuffleboard and watch: - - `Vision/{Camera}/EstimateStatus` should show "Accepted" - - `Vision/{Camera}/EstimateX` and `EstimateY` should roughly match the known position -3. If estimates show "Rejected": - - Single tags beyond 4m are rejected (distance filter) - - High ambiguity tags are rejected (> 0.2 threshold) - - This is working as intended -4. Drive the robot slowly and confirm the pose estimate tracks smoothly -5. Compare `CurrentPoseEstimator` (vision-fused) vs `CurrentPose` (odometry-only) in Shuffleboard - -### Step 5: Iterate on Camera Positions - -Based on testing: -- Adjust camera tilt (pitch) if tags aren't detected at desired ranges -- Adjust camera yaw if the field of view doesn't cover the right area -- Ensure no part of the robot frame blocks the camera view -- After each physical adjustment, **re-measure and update the transforms** - ---- - -## Updating Camera Transforms in Code - -Once you have final measurements, update `VisionSubsystemContext.java`: - -### File: `src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java` - -The default transforms are at lines 54-77. Update them with your measured values: - -```java -// Example: Front-Right camera measured at -// 28cm forward, 22cm right, 30cm up, tilted 12deg down, angled 25deg right -@Builder.Default -private final Transform3d frontRightCameraToRobot = new Transform3d( - new Translation3d(0.28, -0.22, 0.30), - new Rotation3d(0, Math.toRadians(-12), Math.toRadians(-25))); - -// Example: Front-Left camera measured at -// 28cm forward, 22cm left, 30cm up, tilted 12deg down, angled 25deg left -@Builder.Default -private final Transform3d frontLeftCameraToRobot = new Transform3d( - new Translation3d(0.28, 0.22, 0.30), - new Rotation3d(0, Math.toRadians(-12), Math.toRadians(25))); - -// Example: Rear camera measured at -// 25cm backward, centered, 28cm up, tilted 10deg down, facing backward -@Builder.Default -private final Transform3d rearCameraToRobot = new Transform3d( - new Translation3d(-0.25, 0.0, 0.28), - new Rotation3d(0, Math.toRadians(-10), Math.PI)); -``` - -After updating, redeploy: `.\gradlew.bat deploy` - ---- - -## Validating Vision Pose Estimation - -### Using AdvantageScope - -AdvantageScope gives you the best visualization of whether your camera transforms are correct. - -1. **Plug a USB stick** (FAT32 formatted) into the roboRIO USB port -2. Run the robot for a few minutes, driving near AprilTags -3. Retrieve the USB stick - logs are in `FRC_LOGS/` folder -4. Open the `.wpilog` file in AdvantageScope - -In AdvantageScope: -- Add `Drivetrain/Pose` to the **Field 2D** view (odometry-only pose) -- Add `CurrentPoseEstimator` (vision-fused pose) -- If the vision-fused pose jumps wildly, the camera transforms may be wrong -- If the vision-fused pose is smooth and corrects drift, the calibration is good - -### Using Shuffleboard Live - -Key values to monitor during a calibration run: - -| NetworkTables Key | What to Look For | -|-------------------|------------------| -| `Vision/Status` | "Tracking (FR+FL+Rear)" when all cameras see tags | -| `Vision/TotalTagsDetected` | Number of tags currently visible | -| `Vision/FrontRightCamera/EstimateStatus` | "Accepted" means pose was used | -| `Vision/StdDev/XY` | Lower = more trusted. Multi-tag should be ~0.5-1.0 | -| `Vision/TagCount` | 2+ tags = multi-tag mode (more accurate) | -| `Vision/AvgDistance` | Distance to detected tags in meters | -| `CurrentPoseEstimator` | Robot pose with vision corrections applied | - -### Signs of Good Calibration - -- Vision-fused pose (`CurrentPoseEstimator`) is stable and doesn't jump -- Pose corrections are small and smooth when tags become visible -- Multi-tag estimates (2+ tags) produce especially stable poses -- Robot pose on the AdvantageScope field matches physical position - -### Signs of Bad Calibration - -- Pose jumps wildly when vision estimates are accepted -- Robot appears to be in the wrong location on the field visualization -- `EstimateStatus` frequently shows "Rejected" -- Large discrepancy between odometry-only and vision-fused poses - ---- - -## Monitoring & Telemetry - -### Telemetry Level - -The deployed code defaults to `LAB` level, which captures detailed data useful for testing. This is configured in `src/main/deploy/telemetry.properties`: - -```properties -telemetry.level=LAB -``` - -You can change the level live via Shuffleboard by editing `Telemetry/Level`. - -### Key Telemetry to Watch - -#### Drivetrain Health -- `Drivetrain/Pose` - Odometry-only robot position -- `CurrentPoseEstimator` - Vision-fused position -- `Drivetrain/Heading` - Current gyro heading - -#### Vision Health -- `Vision/Status` - Quick overview of camera status -- `Vision/{Camera}/Connected` - Per-camera connection status -- `Vision/{Camera}/TargetCount` - How many tags each camera sees -- `Vision/{Camera}/DetectedTags` - Which tag IDs are visible - -#### Turret Tracker (for hub targeting) -- `TurretTracker/AngleDeg` - Computed aim angle -- `TurretTracker/DistanceM` - Distance to target hub -- `TurretTracker/Mode` - SHOOTING or PASSING mode - -### Log Collection - -For post-session analysis: - -1. **USB stick** (recommended): Insert FAT32 USB into roboRIO before testing - - Logs auto-save to `FRC_LOGS/` on the stick - - File format: `FRC_YYYYMMDD_HHMMSS.wpilog` -2. **Internal storage** (fallback): Logs go to `/home/lvuser/logs/` - - SSH in to retrieve: `scp admin@10.25.34.2:/home/lvuser/logs/*.wpilog .` - -Open logs in **AdvantageScope** for full replay with field visualization. - ---- - -## Troubleshooting - -### Robot Code Won't Start - -| Symptom | Likely Cause | Fix | -|---------|-------------|-----| -| Robot Code light stays red | Java exception on startup | Check DS console for stack trace | -| CAN errors in DS console | Wrong CAN IDs for test bot | Update `Constants.java` Port class | -| "HAL: Resource already allocated" | DIO channel conflict | Check encoder DIO assignments | - -### Cameras Not Detected - -| Symptom | Likely Cause | Fix | -|---------|-------------|-----| -| `Connected = false` for all cameras | Coprocessor not on network | Check Ethernet, ping coprocessor IP | -| `Connected = false` for one camera | Camera name mismatch | Check PhotonVision UI > camera name matches exactly | -| Camera connected but no targets | Pipeline not active | Check PhotonVision UI > pipeline is running (green) | -| Intermittent detection | Poor lighting or focus | Adjust exposure in PhotonVision UI | - -### Swerve Drive Issues - -| Symptom | Likely Cause | Fix | -|---------|-------------|-----| -| Wheels point wrong direction | Turn encoder offsets wrong for test bot | Re-measure offsets (see code changes) | -| Robot drives sideways | Module assignment mismatch | Verify CAN IDs match physical positions | -| Robot spins in circles | Gyro not calibrated | Wait 10 sec after boot, press B to reset | -| Very slow or no movement | Right trigger not pressed | Right trigger = gas pedal | - -### Vision Pose Estimation Issues - -| Symptom | Likely Cause | Fix | -|---------|-------------|-----| -| Pose jumps wildly | Camera transform is wrong | Re-measure physical camera positions | -| All estimates rejected | Tags too far or too ambiguous | Move closer, add more tags | -| Estimates only from one camera | Other cameras misconfigured | Check each camera individually | - ---- - -## Code Changes Checklist - -These are changes you **may** need to make before or during the test bot deployment. - -### Required If Test Bot Has Different CAN IDs - -Update motor CAN IDs in `Constants.java` (`Port` class, lines 107-130): - -```java -public static class Port { - public static final int REAR_LEFT_TURN_CHANNEL = 1; // Update if different - public static final int REAR_LEFT_DRIVE_CHANNEL = 2; // Update if different - public static final int FRONT_LEFT_DRIVE_CHANNEL = 3; // Update if different - // ... etc -} -``` - -### Required: Measure Turn Encoder Offsets for Test Bot - -The current offsets are for the 2025 robot. The test bot will have different values. - -**How to measure offsets:** - -1. Deploy the code with all offsets set to `0`: - ```java - // In Constants.java, temporarily: - public class RobotVersion2025 extends RobotVersionConstants { - public static final double flTurnEncoderOffset = 0; - public static final double frTurnEncoderOffset = 0; - public static final double rlTurnEncoderOffset = 0; - public static final double rrTurnEncoderOffset = 0; - } - ``` -2. Deploy and enable the robot -3. Physically straighten all wheels to point forward (use a straight edge) -4. Read the raw encoder values from Shuffleboard for each module -5. The offset = the raw value when wheels are straight forward -6. Update the constants and redeploy - -### Required: Update Camera Transforms After Measurement - -As described in [Updating Camera Transforms in Code](#updating-camera-transforms-in-code), update `VisionSubsystemContext.java` with measured camera positions. - -### Optional: Starting With Fewer Than 3 Cameras - -If you only have 1 or 2 cameras to start with, the code handles this gracefully - unconnected cameras simply show `Connected = false` and no data. The pose estimator only uses cameras that provide valid data. - -No code changes needed - just physically connect however many cameras you have. - -### Optional: Adjusting Vision Tuning Parameters - -If vision estimates are too noisy or too conservative, adjust in `VisionSubsystemContext.java`: - -```java -// Reject estimates from tags further than this (meters) -private final double maxPoseEstimationDistance = 4.0; // Increase for larger test area - -// Reject single-tag estimates with ambiguity above this -private final double poseAmbiguityThreshold = 0.2; // Increase to accept more, decrease for stricter - -// Trust level: lower = more trusted -private final double singleTagStdDevFactor = 4.0; // Single tag trust (higher = less trust) -private final double multiTagStdDevFactor = 0.5; // Multi tag trust (lower = more trust) -``` - -### Optional: Telemetry Level for Verbose Debugging - -If you need maximum diagnostic data during calibration, edit `src/main/deploy/telemetry.properties`: - -```properties -telemetry.level=VERBOSE -``` - -This logs per-tag yaw/pitch/area/skew data for every detected AprilTag. Useful during calibration, but generates large log files. - ---- - -## Quick Reference: Deploy Cycle - -``` -1. Make code changes -2. Build: .\gradlew.bat build -3. Deploy: .\gradlew.bat deploy -4. Open Driver Station -5. Open Shuffleboard (connect to 10.25.34.2) -6. Enable robot (TeleOp for driving, Test for diagnostics) -7. Monitor telemetry -8. Disable robot -9. Retrieve USB logs if needed -10. Repeat -``` - -## Quick Reference: Camera Names - -| Code Name | PhotonVision NetworkTables Name | -|-----------|-------------------------------| -| Front-Right Camera | `photonvision-front-right` | -| Front-Left Camera | `photonvision-front-left` | -| Rear Camera | `photonvision-rear` | - -## Quick Reference: Controller Layout - -``` -DRIVER (USB 0): - Left Stick = Drive (translation) - Right Stick = Rotate - Right Trigger = Gas (acceleration) - Left Trigger = Half-speed (hold > 50%) - B Button = Reset gyro - R-Stick Click = Wheel lock toggle - -MANIPULATOR (USB 1): - Left Bumper = Fuel intake (hold) - Right Bumper = Fuel launch (hold: 1s spinup then launch) - X Button = Fuel eject (hold) -``` diff --git a/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java b/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java index 3bc8543..8db36c9 100644 --- a/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java +++ b/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java @@ -77,10 +77,19 @@ public class TurretTracker extends SubsystemBase { @Getter private boolean targetInRange = false; - // Distance from robot to the active target in meters. + // Horizontal distance from robot to the active target in meters (2D, X/Y only). + @Getter + private double horizontalDistanceMeters = 0.0; + + // 3D distance from turret to the target, accounting for height difference (meters). @Getter private double distanceToTargetMeters = 0.0; + // Elevation angle to the target in degrees (positive = upward, 0 = flat). + // In passing mode this is always 0 (flat lob trajectory). + @Getter + private double elevationAngleDegrees = 0.0; + // The currently resolved target position (hub center or passing target). @Getter private Translation2d activeTarget = new Translation2d(); @@ -92,7 +101,7 @@ public class TurretTracker extends SubsystemBase { // Visualization: AdvantageScope via StructPublisher private final StructPublisher aimPose3dPublisher; private final StructPublisher targetPose3dPublisher; - private final StructArrayPublisher aimLinePublisher; + private final StructArrayPublisher aimLinePublisher; public TurretTracker(final TurretTrackerContext context, final Drivetrain drivetrain) { this.context = requireNonNull(context, "TurretTrackerContext cannot be null"); @@ -129,7 +138,7 @@ public TurretTracker(final TurretTrackerContext context, final Drivetrain drivet this.targetPose3dPublisher = nti.getStructTopic("TurretTracker/TargetPose3d", Pose3d.struct).publish(); this.aimLinePublisher = - nti.getStructArrayTopic("TurretTracker/AimLine", Pose2d.struct).publish(); + nti.getStructArrayTopic("TurretTracker/AimLine", Pose3d.struct).publish(); // Register telemetry Telemetry.registerSubsystem(TELEMETRY_PREFIX, this::captureTelemetry); @@ -188,10 +197,25 @@ public void periodic() { // Resolve the active target based on tracking mode activeTarget = (trackingMode == TrackingMode.PASSING) ? computePassingTarget(robotPose, hubCenter) : hubCenter; - // Calculate distance to active target + // Calculate horizontal distance to active target (2D) double dx = activeTarget.getX() - robotPose.getX(); double dy = activeTarget.getY() - robotPose.getY(); - distanceToTargetMeters = Math.sqrt(dx * dx + dy * dy); + horizontalDistanceMeters = Math.sqrt(dx * dx + dy * dy); + + // Calculate height difference and 3D distance + double targetZ = (trackingMode == TrackingMode.SHOOTING) + ? context.getShootingTargetHeightMeters() + : context.getPassingTargetHeightMeters(); + double dz = targetZ - context.getTurretHeightMeters(); + distanceToTargetMeters = Math.sqrt(dx * dx + dy * dy + dz * dz); + + // Calculate elevation angle (positive = upward, 0 = flat) + // For passing mode, force flat (0°) since we lob over obstacles + if (trackingMode == TrackingMode.PASSING) { + elevationAngleDegrees = 0.0; + } else { + elevationAngleDegrees = Units.radiansToDegrees(Math.atan2(dz, horizontalDistanceMeters)); + } // Calculate field-relative angle from robot to active target double fieldAngleRad = Math.atan2(dy, dx); @@ -309,30 +333,41 @@ private void updateAdvantageScope(Pose2d robotPose, Translation2d hubCenter) { // Field-relative aim direction double aimFieldAngleRad = robotPose.getRotation().getRadians() + Units.degreesToRadians(turretAngleDegrees); - // Aim pose at robot position, pointed toward hub center + // Aim pose at robot position, pointed toward active target with elevation pitch + double elevPitchRad = Units.degreesToRadians(elevationAngleDegrees); Pose3d aimPose = new Pose3d( robotPose.getX(), robotPose.getY(), context.getTurretHeightMeters(), - new Rotation3d(0, 0, aimFieldAngleRad)); + new Rotation3d(0, -elevPitchRad, aimFieldAngleRad)); aimPose3dPublisher.set(aimPose); - // Hub center as a Pose3d (Z = turret height for visual alignment) - Pose3d targetPose = - new Pose3d(hubCenter.getX(), hubCenter.getY(), context.getTurretHeightMeters(), new Rotation3d()); + // Active target as a Pose3d at the actual target height + double activeTargetZ = (trackingMode == TrackingMode.SHOOTING) + ? context.getShootingTargetHeightMeters() + : context.getPassingTargetHeightMeters(); + Pose3d targetPose = new Pose3d(hubCenter.getX(), hubCenter.getY(), activeTargetZ, new Rotation3d()); targetPose3dPublisher.set(targetPose); - // Aim line: array of 2 Pose2d (start at robot, end at aim vector endpoint) - double endX = robotPose.getX() + context.getAimVectorLengthMeters() * Math.cos(aimFieldAngleRad); - double endY = robotPose.getY() + context.getAimVectorLengthMeters() * Math.sin(aimFieldAngleRad); - - Pose2d[] aimLine = new Pose2d[] { - robotPose, new Pose2d(endX, endY, new Rotation2d(aimFieldAngleRad)), + // Aim line: array of 2 Pose3d from turret to aim vector endpoint. + // In shooting mode the line pitches upward toward the hub intake height; + // in passing mode it stays flat (elevation = 0). + double turretZ = context.getTurretHeightMeters(); + double elevationRad = Units.degreesToRadians(elevationAngleDegrees); + double aimLength = context.getAimVectorLengthMeters(); + + // Horizontal projection of the aim vector (shortened by pitch) + double horizontalLength = aimLength * Math.cos(elevationRad); + double endX = robotPose.getX() + horizontalLength * Math.cos(aimFieldAngleRad); + double endY = robotPose.getY() + horizontalLength * Math.sin(aimFieldAngleRad); + double endZ = turretZ + aimLength * Math.sin(elevationRad); + + // Rotation3d: roll=0, pitch=-elevation (WPILib pitch is nose-down positive), yaw=aim heading + Rotation3d aimRot = new Rotation3d(0, -elevationRad, aimFieldAngleRad); + Pose3d[] aimLine = new Pose3d[] { + new Pose3d(robotPose.getX(), robotPose.getY(), turretZ, aimRot), new Pose3d(endX, endY, endZ, aimRot), }; aimLinePublisher.set(aimLine); - - // Also record for DataLog (AdvantageScope replay) - Telemetry.recordPoses(TELEMETRY_PREFIX + "/AimLine", aimLine, TelemetryLevel.MATCH); } private void captureTelemetry(String prefix) { @@ -340,12 +375,16 @@ private void captureTelemetry(String prefix) { Telemetry.record(prefix + "/AngleDeg", turretAngleDegrees, TelemetryLevel.MATCH); Telemetry.record(prefix + "/InRange", targetInRange, TelemetryLevel.MATCH); Telemetry.record(prefix + "/DistanceM", distanceToTargetMeters, TelemetryLevel.MATCH); + Telemetry.record(prefix + "/HorizontalDistM", horizontalDistanceMeters, TelemetryLevel.MATCH); + Telemetry.record(prefix + "/ElevationDeg", elevationAngleDegrees, TelemetryLevel.MATCH); Telemetry.record(prefix + "/Mode", trackingMode.name(), TelemetryLevel.MATCH); // Publish to NT for live dashboard Telemetry.publish(prefix + "/AngleDeg", turretAngleDegrees, TelemetryLevel.MATCH); Telemetry.publish(prefix + "/InRange", targetInRange, TelemetryLevel.MATCH); Telemetry.publish(prefix + "/DistanceM", distanceToTargetMeters, TelemetryLevel.MATCH); + Telemetry.publish(prefix + "/HorizontalDistM", horizontalDistanceMeters, TelemetryLevel.MATCH); + Telemetry.publish(prefix + "/ElevationDeg", elevationAngleDegrees, TelemetryLevel.MATCH); Telemetry.publish(prefix + "/Mode", trackingMode.name(), TelemetryLevel.MATCH); // LAB level - detailed tracking data @@ -358,7 +397,9 @@ private void captureTelemetry(String prefix) { String modeLabel = trackingMode == TrackingMode.PASSING ? "Passing" : "Hub Center"; String status = targetInRange - ? String.format("Tracking %s (%.1f deg, %.1fm)", modeLabel, turretAngleDegrees, distanceToTargetMeters) + ? String.format( + "Tracking %s (%.1f deg, %.1f elev, %.1fm)", + modeLabel, turretAngleDegrees, elevationAngleDegrees, distanceToTargetMeters) : String.format("Out of Range (%.1f deg)", rawAngleDegrees); Telemetry.publish(prefix + "/Status", status, TelemetryLevel.MATCH); } diff --git a/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java b/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java index 09feaf4..6971cbf 100644 --- a/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java +++ b/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java @@ -20,9 +20,26 @@ public class TurretTrackerContext { /** * Height of the turret above ground for 3D visualization (meters). + * 19 inches = 0.4826m. */ @Builder.Default - private final double turretHeightMeters = 0.5; + private final double turretHeightMeters = 0.4826; + + /** + * Height of the hub intake opening above ground (meters). + * 72 inches = 1.8288m. Used in shooting mode to compute elevation angle + * and 3D distance for motor speed derivation. + */ + @Builder.Default + private final double shootingTargetHeightMeters = 1.8288; + + /** + * Height of the passing target above ground (meters). + * Passing uses a lob trajectory, so elevation is computed as 0 (flat) + * rather than aiming down at the ground. + */ + @Builder.Default + private final double passingTargetHeightMeters = 0.0; /** * Length of the aim vector line drawn in visualizations (meters). diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 4e932a8..820e13f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -5,7 +5,9 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; 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.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -81,10 +83,10 @@ public interface VisionMeasurementConsumer { private PhotonCameraSim leftSideCameraSim; // FOV visualization publishers (simulation only) - private StructArrayPublisher frontRightFovPublisher; - private StructArrayPublisher frontLeftFovPublisher; - private StructArrayPublisher rightSideFovPublisher; - private StructArrayPublisher leftSideFovPublisher; + private StructArrayPublisher frontRightFovPublisher; + private StructArrayPublisher frontLeftFovPublisher; + private StructArrayPublisher rightSideFovPublisher; + private StructArrayPublisher leftSideFovPublisher; private Mechanism2d cameraLayoutMech; /** @@ -214,13 +216,13 @@ private SimCameraProperties createSimCameraProperties() { private void initializeFovVisualization() { NetworkTableInstance nti = NetworkTableInstance.getDefault(); - frontRightFovPublisher = nti.getStructArrayTopic("Vision/FrontRight/FOVCone", Pose2d.struct) + frontRightFovPublisher = nti.getStructArrayTopic("Vision/FrontRight/FOVCone", Pose3d.struct) .publish(); - frontLeftFovPublisher = nti.getStructArrayTopic("Vision/FrontLeft/FOVCone", Pose2d.struct) + frontLeftFovPublisher = nti.getStructArrayTopic("Vision/FrontLeft/FOVCone", Pose3d.struct) .publish(); - rightSideFovPublisher = nti.getStructArrayTopic("Vision/RightSide/FOVCone", Pose2d.struct) + rightSideFovPublisher = nti.getStructArrayTopic("Vision/RightSide/FOVCone", Pose3d.struct) .publish(); - leftSideFovPublisher = nti.getStructArrayTopic("Vision/LeftSide/FOVCone", Pose2d.struct) + leftSideFovPublisher = nti.getStructArrayTopic("Vision/LeftSide/FOVCone", Pose3d.struct) .publish(); // Mechanism2d: top-down camera layout (robot center, 4 directional lines) @@ -233,10 +235,10 @@ private void initializeFovVisualization() { center.append(new MechanismLigament2d("frontRightCam", 30, 90 - 30, 2, new Color8Bit(Color.kOrange))); // Front-left at yaw=+30deg: mechanism angle = 90 + 30 = 120 center.append(new MechanismLigament2d("frontLeftCam", 30, 90 + 30, 2, new Color8Bit(Color.kYellow))); - // Right-side at yaw=-90deg: mechanism angle = 90 + (-90) = 0 - center.append(new MechanismLigament2d("rightSideCam", 30, 0, 2, new Color8Bit(Color.kCyan))); - // Left-side at yaw=+90deg: mechanism angle = 90 + 90 = 180 - center.append(new MechanismLigament2d("leftSideCam", 30, 180, 2, new Color8Bit(Color.kMagenta))); + // Right-side at yaw=-120deg: mechanism angle = 90 + (-120) = -30 + center.append(new MechanismLigament2d("rightSideCam", 30, -30, 2, new Color8Bit(Color.kCyan))); + // Left-side at yaw=+120deg: mechanism angle = 90 + 120 = 210 + center.append(new MechanismLigament2d("leftSideCam", 30, 210, 2, new Color8Bit(Color.kMagenta))); Telemetry.putData("Vision/CameraLayout", cameraLayoutMech); } @@ -422,8 +424,9 @@ public void simulationPeriodic() { /** * Computes field-relative FOV cone edges for each camera and publishes - * as Pose2d arrays for AdvantageScope 2D field overlay. - * Each FOV cone is a 3-point V shape: [left edge, camera position, right edge]. + * as Pose3d arrays for AdvantageScope 3D field overlay at the camera's + * mounted height. Each FOV cone is a 3-point V shape: + * [left edge, camera position, right edge]. */ private void updateFovVisualization(Pose2d robotPose) { double rayLength = context.getFovVisualizationRayLength(); @@ -437,11 +440,12 @@ private void updateFovVisualization(Pose2d robotPose) { } /** - * Publishes a single camera's FOV cone as a V-shaped Pose2d array. - * Projects the camera position and FOV edges onto the field coordinate system. + * Publishes a single camera's FOV cone as a V-shaped Pose3d array. + * Projects the camera position and FOV edges onto the field coordinate system + * at the camera's mounted Z height. */ private void publishCameraFov( - StructArrayPublisher publisher, + StructArrayPublisher publisher, Pose2d robotPose, Transform3d cameraToRobot, double halfFovRad, @@ -454,6 +458,7 @@ private void publishCameraFov( double sinH = Math.sin(robotHeading); double camX = robotPose.getX() + cameraToRobot.getX() * cosH - cameraToRobot.getY() * sinH; double camY = robotPose.getY() + cameraToRobot.getX() * sinH + cameraToRobot.getY() * cosH; + double camZ = cameraToRobot.getZ(); // Camera heading in field coordinates (robot heading + camera yaw) double cameraYaw = cameraToRobot.getRotation().getZ(); @@ -468,11 +473,15 @@ private void publishCameraFov( double rightX = camX + rayLength * Math.cos(rightAngle); double rightY = camY + rayLength * Math.sin(rightAngle); - Pose2d leftEdge = new Pose2d(leftX, leftY, new Rotation2d(leftAngle)); - Pose2d camPose = new Pose2d(camX, camY, new Rotation2d(camHeading)); - Pose2d rightEdge = new Pose2d(rightX, rightY, new Rotation2d(rightAngle)); + Rotation3d leftRot = new Rotation3d(0, 0, leftAngle); + Rotation3d camRot = new Rotation3d(0, 0, camHeading); + Rotation3d rightRot = new Rotation3d(0, 0, rightAngle); - publisher.set(new Pose2d[] {leftEdge, camPose, rightEdge}); + Pose3d leftEdge = new Pose3d(leftX, leftY, camZ, leftRot); + Pose3d camPose = new Pose3d(camX, camY, camZ, camRot); + Pose3d rightEdge = new Pose3d(rightX, rightY, camZ, rightRot); + + publisher.set(new Pose3d[] {leftEdge, camPose, rightEdge}); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java index eb6ddb8..49165b0 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java @@ -55,42 +55,42 @@ public class VisionSubsystemContext { /** * Transform from robot center to front-right camera optical center. * Mounted on the front-right bumper corner, angled 30deg outward to the right. - * Position: X=+0.30m forward, Y=-0.25m right, Z=+0.25m up. + * Position: X=+0.30m forward, Y=-0.25m right, Z=+0.2286m up (9in). * Rotation: pitch=-15deg (tilted down), yaw=-30deg (angled right). */ @Builder.Default private final Transform3d frontRightCameraToRobot = new Transform3d( - new Translation3d(0.30, -0.25, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-30))); + new Translation3d(0.30, -0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-30))); /** * Transform from robot center to front-left camera optical center. * Mounted on the front-left bumper corner, angled 30deg outward to the left. - * Position: X=+0.30m forward, Y=+0.25m left, Z=+0.25m up. + * Position: X=+0.30m forward, Y=+0.25m left, Z=+0.2286m up (9in). * Rotation: pitch=-15deg (tilted down), yaw=+30deg (angled left). */ @Builder.Default private final Transform3d frontLeftCameraToRobot = new Transform3d( - new Translation3d(0.30, 0.25, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(30))); + new Translation3d(0.30, 0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(30))); /** * Transform from robot center to right-side camera optical center. - * Mounted on the right side of the robot, facing perpendicular to the right. - * Position: X=0.0m (centered fore-aft), Y=-0.30m right, Z=+0.25m up. - * Rotation: pitch=-15deg (tilted down), yaw=-90deg (facing right). + * Adjacent to the front-right camera, angled 120deg to the right. + * Position: X=+0.30m forward, Y=-0.25m right, Z=+0.2286m up (9in). + * Rotation: pitch=-15deg (tilted down), yaw=-120deg. */ @Builder.Default private final Transform3d rightSideCameraToRobot = new Transform3d( - new Translation3d(0.0, -0.30, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-90))); + new Translation3d(0.30, -0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-120))); /** * Transform from robot center to left-side camera optical center. - * Mounted on the left side of the robot, facing perpendicular to the left. - * Position: X=0.0m (centered fore-aft), Y=+0.30m left, Z=+0.25m up. - * Rotation: pitch=-15deg (tilted down), yaw=+90deg (facing left). + * Adjacent to the front-left camera, angled 120deg to the left. + * Position: X=+0.30m forward, Y=+0.25m left, Z=+0.2286m up (9in). + * Rotation: pitch=-15deg (tilted down), yaw=+120deg. */ @Builder.Default private final Transform3d leftSideCameraToRobot = new Transform3d( - new Translation3d(0.0, 0.30, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(90))); + new Translation3d(0.30, 0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(120))); /** * Whether to enable simulation features (VisionSystemSim)