From 1e93d12453570d486c93244e28e64602be2f4627 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:05:27 +0800 Subject: [PATCH 1/7] docs: design spec for BIOBUZZ teleop robot code --- .../specs/2026-09-20-biobuzz-teleop-design.md | 199 ++++++++++++++++++ 1 file changed, 199 insertions(+) create mode 100644 docs/superpowers/specs/2026-09-20-biobuzz-teleop-design.md diff --git a/docs/superpowers/specs/2026-09-20-biobuzz-teleop-design.md b/docs/superpowers/specs/2026-09-20-biobuzz-teleop-design.md new file mode 100644 index 0000000..53841b0 --- /dev/null +++ b/docs/superpowers/specs/2026-09-20-biobuzz-teleop-design.md @@ -0,0 +1,199 @@ +# BIOBUZZ TeleOp robot code, design + +Lebob Robotics, FTC 29550 · BIOBUZZ 2026/27 · v1.0, 20 Sep 2026 + +## Purpose + +This document describes the robot code for the WA Qualifier. It covers the +drive, intake, indexer, shooter, odometry and camera for driver control. There +is no autonomous in this pass. The code is structured so Pedro Pathing can be +added later without rewriting the mechanisms. + +## The robot + +- Mecanum drive, four goBILDA Yellow Jacket 435 RPM motors. +- Intake: one front roller of compliant wheels, belt driven from a 5.2:1 + (1150 RPM) Yellow Jacket. +- Indexer: an inclined ramp of compliant wheels that carries balls to the + shooter, driven by one 5.2:1 (1150 RPM) Yellow Jacket. +- Shooter: two flywheels, one 1:1 (6000 RPM, 28 ticks per rev) Yellow Jacket + each. +- goBILDA Pinpoint with two dead-wheel pods on I2C. +- One UVC webcam on the Control Hub. +- REV Control Hub plus one Expansion Hub. Eight motors fills both hubs. + +## What the game asks of the code + +Rule references are to the BIOBUZZ Competition Manual V1 with Team Update 01. + +- Score by launching Pollen and Nectar into the upward-facing Cell of our + Hive. A Hive Tip is 20 points. The Cell opening is 20 in wide by 14 in tall + with its bottom edge 53.5 in above the tiles. +- Each Cell carries a four-tag AprilTag cluster whose origin is the centre of + the opening. The Cells move, so tags are for aiming only (SDK v12 release + notes). +- A robot may control at most four balls at once (G407). The mechanism + enforces this. The code does not count balls. +- Robot Controller must be one Control Hub with at most one Expansion Hub + (R701). One webcam is allowed (R708). FTC Dashboard and other Wi-Fi + streaming tools are banned at events (R704), so tuning happens through + Driver Station telemetry. + +## Step 0: SDK v12.0 + +FTC released SDK v12.0 on 12 September 2026 as the BIOBUZZ season release. The +repo is on the offseason v11.2.1. We move to v12.0 before writing new code, +because the camera work needs the BIOBUZZ tag library and cluster API that +only v12 has, and because the season release is what inspectors expect. + +The move replaces the `FtcRobotController` module, `build.gradle`, +`build.common.gradle`, `build.dependencies.gradle` and the Gradle wrapper with +the v12.0 versions, then reapplies our two changes: the FTCLib lines in +`TeamCode/build.gradle`, and any README or CI package pins that the new +`build.common.gradle` moves. FTCLib 2.1.1 targets the same SDK APIs, so it is +expected to compile unchanged. If it does not, the plan stops and we decide +whether to drop FTCLib or patch around it. + +## Architecture + +We keep the FTCLib command-based layout that is already in the repo. One +`OpMode` (`Main`) owns a `Robot`. `Robot` builds one subsystem per mechanism, +runs the `CommandScheduler` each loop, and maps the gamepad. Each subsystem is +one file under `subsystems/` and exposes a handful of methods. No interfaces, +no factories. + +A `Constants` class holds the hardware config names, motor directions and +tuning values (shooter RPM, aim gain, tolerances). It exists because Pedro +Pathing will need the same motor names and directions, and because tuning +numbers need one home the drive team can find. + +Both hubs are set to `BulkCachingMode.AUTO` in the `Robot` constructor so the +eight motor reads cost two bus transactions per loop. + +### Subsystems + +**MecanumDriveSubsystem** (existing). Unchanged behaviour. The mixing maths +moves into a static `MecanumKinematics.mix(forward, right, rotate)` that +returns four normalised powers, so it can be unit tested on the JVM and so a +Pedro Pathing follower can replace the subsystem later without touching the +maths. + +**OdometrySubsystem** (existing). Unchanged. The pod offsets in `init()` are a +tuning task, done on the robot with the goBILDA setup procedure. Pedro Pathing +ships its own Pinpoint localiser, so when it arrives this subsystem either +stays for teleop heading or is removed. + +**IntakeSubsystem.** One `DcMotorEx`, `RUN_WITHOUT_ENCODER`, zero-power +`FLOAT`. Methods: `run()`, `reverse()`, `stop()`. + +**IndexerSubsystem.** One `DcMotorEx`, `RUN_WITHOUT_ENCODER`, zero-power +`BRAKE` so held balls do not roll back. Methods: `feed()`, `reverse()`, +`stop()`. + +**ShooterSubsystem.** Two `DcMotorEx` in `RUN_USING_ENCODER`, one reversed so +both wheels throw forward, zero-power `FLOAT` so the wheels spin down on their +own. Velocity control uses the hub's built-in velocity PIDF through +`setVelocityPIDFCoefficients`, with the gains in `Constants`. One shared RPM +setpoint. Methods: `spinUp()`, `idle()`, `stop()`, `atSpeed()` (both wheels +within `SHOOTER_TOLERANCE_RPM` of the setpoint), `getVelocityRpm()` for +telemetry. The conversion from RPM to encoder ticks per second is a static +function so it can be unit tested. Starting values: 3500 RPM setpoint, 100 RPM +tolerance, SDK default PIDF. All three are tuned on the robot. + +**VisionSubsystem.** One webcam through `VisionPortal` with the v12 +`AprilTagProcessor` using `getCurrentGameTagLibrary()`. It keeps the latest +detection of our alliance's Cell cluster, chosen by tag ID from the library, +and exposes `hasTarget()` and `getBearingDeg()`. Cluster detections are used +where the SDK provides them, because their pose points at the opening centre +even when only one member tag is visible. The live stream is off during a match +loop to save CPU. Which IDs belong to which alliance is read from the v12 tag +library during implementation. + +### Controls + +One driver on gamepad1. Alliance colour is chosen during `init` with D-pad +left (red) or right (blue), shown on telemetry, and defaults to red. + +| Input | Action | +| --- | --- | +| Left stick | Translate (field-centric by default) | +| Right stick X | Rotate | +| Left bumper (hold) | Robot-centric translate | +| A | Zero heading | +| Right trigger (hold) | Intake and indexer run | +| Left trigger (hold) | Intake and indexer reverse | +| Right bumper (toggle) | Shooter spin up / idle | +| X (hold) | Fire: indexer feeds only while the shooter is at speed | +| Y (hold) | Aim assist: rotation comes from the tag bearing, not the stick | + +Aim assist is a proportional controller on bearing with a deadband and an +output clamp, gains in `Constants`. With no target visible the right stick +rotates as normal. Triggers count as pressed above 0.2. + +### Telemetry + +Each loop: pose, alliance, shooter target and measured RPM per wheel, at-speed +flag, tag bearing or "no target", hub battery voltage. Nothing else, so the +Driver Station stays readable. + +### Hardware configuration names + +| Config name | Type | Mechanism | +| --- | --- | --- | +| `front_left_drive` | goBILDA 5202/3/4 | Drive | +| `front_right_drive` | goBILDA 5202/3/4 | Drive | +| `back_left_drive` | goBILDA 5202/3/4 | Drive | +| `back_right_drive` | goBILDA 5202/3/4 | Drive | +| `intake` | goBILDA 5202/3/4 | Intake | +| `indexer` | goBILDA 5202/3/4 | Indexer | +| `shooter_left` | goBILDA 5202/3/4 | Shooter | +| `shooter_right` | goBILDA 5202/3/4 | Shooter | +| `pinpoint` | goBILDA Pinpoint (I2C) | Odometry | +| `Webcam 1` | Webcam | Vision | + +This table goes into the README with a ports column, filled in by whoever +wires the hubs, because the PR template already tells contributors to keep it +current. + +## Testing + +Robot code cannot run off the robot, so verification is in two parts. + +**JVM unit tests** for the pure maths only: mecanum mixing (straight, strafe, +rotate, saturation) and RPM to ticks-per-second. These run in CI with +`./gradlew :TeamCode:testDebugUnitTest` and need JUnit 4 as a +`testImplementation` dependency. Nothing with an SDK import gets a unit test. + +**On-robot checklist**, one per subsystem, recorded in the PR under "Tested on +the robot": + +1. Drive: each motor spins forward on positive power, robot drives straight, + strafes right on stick right, field-centric holds direction after a spin. +2. Odometry: Pinpoint LED green, X grows driving forward, Y grows driving + left, heading grows turning anticlockwise, spin in place moves X/Y under + 100 mm, return to start reads under 10 mm. +3. Intake and indexer: run, reverse, stop, indexer holds a ball when stopped. +4. Shooter: reaches setpoint, telemetry RPM within tolerance, at-speed flag + flips, fires a Pollen into the Cell from the practice spot. +5. Vision: telemetry shows a bearing when a Cell tag is in view, aim assist + turns the robot toward it and settles. + +## Pedro Pathing later + +Pedro Pathing brings its own `Follower` that owns the drive motors and a +Pinpoint localiser. When it is added: the follower's teleop drive replaces the +`drive.drive()` call in `Robot.periodic()`, `Constants` supplies the same motor +names and directions to Pedro's constants, and autonomous OpModes are new +files. Intake, indexer, shooter and vision do not change. + +## Out of scope + +Autonomous, distance-based shooter speed, ball counting, flower and garden +mechanisms, driver two controls, FTC Dashboard. + +## Open items + +- Pod offsets, shooter setpoint, PIDF and aim gain are measured on the robot. +- Hub port assignments go into the README table when the robot is wired. +- The camera mount position on the robot is not in the CAD yet. Vision code + assumes the camera faces forward and is roughly on the robot centreline. From aa9c039ff3a006516eb2fa59d652c77939a9ec25 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:13:36 +0800 Subject: [PATCH 2/7] docs: implementation plan for BIOBUZZ teleop --- .../plans/2026-09-20-biobuzz-teleop.md | 1174 +++++++++++++++++ 1 file changed, 1174 insertions(+) create mode 100644 docs/superpowers/plans/2026-09-20-biobuzz-teleop.md diff --git a/docs/superpowers/plans/2026-09-20-biobuzz-teleop.md b/docs/superpowers/plans/2026-09-20-biobuzz-teleop.md new file mode 100644 index 0000000..785df4b --- /dev/null +++ b/docs/superpowers/plans/2026-09-20-biobuzz-teleop.md @@ -0,0 +1,1174 @@ +# BIOBUZZ TeleOp Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Driver-controlled robot code for the BIOBUZZ WA Qualifier: mecanum drive with Pinpoint heading, intake, indexer, twin-flywheel shooter with closed-loop velocity, and AprilTag aim assist on the alliance's Hive cell. + +**Architecture:** FTCLib command-based layout already in the repo. One `OpMode` (`Main`) owns a `Robot`, which builds one `SubsystemBase` per mechanism, runs the `CommandScheduler` each loop, and polls a `GamepadEx`. Pure maths (mecanum mixing, RPM conversion) lives in static classes with JVM unit tests. Tuning values and config names live in `Constants`. + +**Tech Stack:** FTC SDK v12.0, FTCLib 2.1.1 (core), Java 8, Gradle 9.1 / AGP 8.13.2, JUnit 4 for JVM tests, goBILDA Pinpoint driver (built into the SDK), VisionPortal + AprilTagProcessor. + +**Spec:** `docs/superpowers/specs/2026-09-20-biobuzz-teleop-design.md` + +## Global Constraints + +- SDK v12.0 (`org.firstinspires.ftc:*:12.0.0`), the BIOBUZZ season release. No new SDK-external dependencies beyond JUnit 4 for tests. +- FTCLib stays at `org.ftclib.ftclib:core:2.1.1`. Drop `org.ftclib.ftclib:vision:2.1.0`; nothing uses it. +- Java 8 source level (`build.common.gradle`, do not edit). +- Config names exactly as in the spec table: `front_left_drive`, `front_right_drive`, `back_left_drive`, `back_right_drive`, `intake`, `indexer`, `shooter_left`, `shooter_right`, `pinpoint`, `Webcam 1`. +- One driver on gamepad1. Controls exactly as the spec table. +- No FTC Dashboard or other Wi-Fi tools (manual R704). Tuning goes through Driver Station telemetry. +- Commit prefixes used in this repo: `feature:`, `docs:`, `chore:`. +- Building needs JDK 17 and the Android SDK packages listed in the README. CI (`.github/workflows/build.yml`) has them. If `./gradlew` fails locally with a missing SDK or wrong JDK, follow the README "Requirements" section or push a branch and read the CI result instead. +- Anything that touches hardware is verified on the robot using the checklist in the spec, and the PR template's "Tested on the robot" section records it. + +--- + +## File map + +Create: +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java`: config names, motor directions, tuning values. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java`: `enum Alliance { RED, BLUE }`. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumKinematics.java`: pure maths for field-centric rotation and wheel mixing. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java`: RPM to ticks-per-second and back. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java` +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java` +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java` +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java` +- `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MecanumKinematicsTest.java` +- `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java` + +Modify: +- `build.dependencies.gradle`: SDK 11.2.1 to 12.0.0. +- `FtcRobotController/src/main/AndroidManifest.xml`: versionName 12.0. +- Six AprilTag sample files under `FtcRobotController/.../external/samples/`: replace with v12.0 copies. +- `TeamCode/build.gradle`: drop ftclib vision, add JUnit. +- `.github/workflows/build.yml`: run unit tests. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java`: add `init_loop()` and `start()`. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java`: build new subsystems, bulk caching, `GamepadEx` controls, telemetry. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java`: use `Constants` and `MecanumKinematics`. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java`: use `Constants` for the config name and pod offsets. +- `README.md`: SDK version, config-name table, on-robot checklist pointer. + +--- + +### Task 1: Move the SDK to v12.0 + +**Files:** +- Modify: `build.dependencies.gradle` +- Modify: `FtcRobotController/src/main/AndroidManifest.xml:5` +- Modify: `FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/{ConceptAprilTag,ConceptAprilTagEasy,ConceptAprilTagLocalization,ConceptAprilTagSwitchableCameras,RobotAutoDriveToAprilTagOmni,RobotAutoDriveToAprilTagTank}.java` +- Modify: `TeamCode/build.gradle:28-30` +- Modify: `README.md` (Requirements section) + +**Interfaces:** +- Produces: the v12 AprilTag classes used in Task 7: `AprilTagDetection` (abstract, has `ftcPose.bearing`), `AprilTagSingleDetection` (`id`, `metadata`), `AprilTagClusterDetection` (`percentClusterFound`, `metadata.name`), `AprilTagGameDatabase.getCurrentGameTagLibrary()`. + +- [ ] **Step 1: Bump the SDK artifact versions** + +In `build.dependencies.gradle` change every `11.2.1` to `12.0.0`: + +```bash +sed -i 's/:11\.2\.1/:12.0.0/g' build.dependencies.gradle +grep -c '12.0.0' build.dependencies.gradle # expect 8 +``` + +- [ ] **Step 2: Bump the manifest version name** + +```bash +sed -i 's/android:versionName="11.2.1"/android:versionName="12.0"/' FtcRobotController/src/main/AndroidManifest.xml +grep versionName FtcRobotController/src/main/AndroidManifest.xml +``` + +Expected: `android:versionName="12.0">`. Leave `versionCode` as it is; v12.0 upstream did not change it. + +- [ ] **Step 3: Replace the six AprilTag samples with the v12.0 copies** + +The v12 cluster API changed these samples. Fetch them straight from the upstream tag so they match the library: + +```bash +D=FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples +for f in ConceptAprilTag ConceptAprilTagEasy ConceptAprilTagLocalization ConceptAprilTagSwitchableCameras RobotAutoDriveToAprilTagOmni RobotAutoDriveToAprilTagTank; do + gh api "repos/FIRST-Tech-Challenge/FtcRobotController/contents/$D/$f.java?ref=v12.0" --jq .content | base64 -d > "$D/$f.java" +done +grep -l "AprilTagSingleDetection" $D/ConceptAprilTag.java # expect a hit +``` + +- [ ] **Step 4: Drop the unused FTCLib vision artifact and add JUnit** + +Edit the `dependencies` block at the bottom of `TeamCode/build.gradle` to read: + +```groovy +dependencies { + implementation project(':FtcRobotController') + implementation 'org.ftclib.ftclib:core:2.1.1' + testImplementation 'junit:junit:4.13.2' +} +``` + +- [ ] **Step 5: Update the README requirement line** + +In `README.md`, change + +``` +- **FIRST Tech Challenge SDK v11.2.1** (already in this repository) +``` + +to + +``` +- **FIRST Tech Challenge SDK v12.0** (already in this repository, the BIOBUZZ season release) +``` + +- [ ] **Step 6: Build** + +```bash +./gradlew :TeamCode:assembleDebug --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. If it fails inside FTCLib with a missing class, stop and report; that is the FTCLib-compatibility risk from the spec and needs a decision before continuing. + +- [ ] **Step 7: Commit** + +```bash +git add build.dependencies.gradle FtcRobotController/src/main/AndroidManifest.xml FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples TeamCode/build.gradle README.md +git commit -m "chore: move to FTC SDK v12.0, the BIOBUZZ season release" +``` + +--- + +### Task 2: Mecanum maths as a tested pure function + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumKinematics.java` +- Test: `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MecanumKinematicsTest.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java` +- Modify: `.github/workflows/build.yml` + +**Interfaces:** +- Produces: `static double[] MecanumKinematics.toRobotRelative(double forward, double right, double headingRadians)` returning `{forward, right}`; `static double[] MecanumKinematics.mix(double forward, double right, double rotate)` returning `{frontLeft, frontRight, backLeft, backRight}` normalised so no entry exceeds 1 in magnitude. + +- [ ] **Step 1: Write the failing tests** + +```java +package org.firstinspires.ftc.teamcode; + +import static org.junit.Assert.assertArrayEquals; + +import org.junit.Test; + +public class MecanumKinematicsTest { + private static final double EPS = 1e-9; + + @Test + public void forwardDrivesAllWheelsForward() { + assertArrayEquals(new double[]{1, 1, 1, 1}, MecanumKinematics.mix(1, 0, 0), EPS); + } + + @Test + public void strafeRightUsesDiagonalPattern() { + assertArrayEquals(new double[]{1, -1, -1, 1}, MecanumKinematics.mix(0, 1, 0), EPS); + } + + @Test + public void rotateClockwiseDrivesLeftForwardRightBack() { + assertArrayEquals(new double[]{1, -1, 1, -1}, MecanumKinematics.mix(0, 0, 1), EPS); + } + + @Test + public void saturationScalesAllWheelsTogether() { + // raw = {3, -1, 1, 1}; divide by 3 + assertArrayEquals(new double[]{1, -1.0 / 3, 1.0 / 3, 1.0 / 3}, MecanumKinematics.mix(1, 1, 1), EPS); + } + + @Test + public void smallInputsAreNotScaledUp() { + assertArrayEquals(new double[]{0.5, 0.5, 0.5, 0.5}, MecanumKinematics.mix(0.5, 0, 0), EPS); + } + + @Test + public void zeroHeadingLeavesInputUnchanged() { + assertArrayEquals(new double[]{0.3, -0.4}, MecanumKinematics.toRobotRelative(0.3, -0.4, 0), EPS); + } + + @Test + public void robotFacingLeftMovesRightToGoFieldForward() { + // Robot rotated 90 degrees anticlockwise. A field-forward command becomes robot-right. + assertArrayEquals(new double[]{0, 1}, MecanumKinematics.toRobotRelative(1, 0, Math.PI / 2), EPS); + } +} +``` + +- [ ] **Step 2: Run the test and watch it fail** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*MecanumKinematicsTest' --no-daemon +``` + +Expected: compile failure, `cannot find symbol MecanumKinematics`. + +- [ ] **Step 3: Implement** + +```java +package org.firstinspires.ftc.teamcode; + +/** Pure maths for a four-wheel mecanum base. No hardware, so it runs in JVM unit tests. */ +public final class MecanumKinematics { + private MecanumKinematics() {} + + /** + * Rotates a field-relative (forward, right) command into the robot frame. + * + * @param headingRadians robot heading, anticlockwise positive, 0 = facing field-forward + * @return {forward, right} in the robot frame + */ + public static double[] toRobotRelative(double forward, double right, double headingRadians) { + double cos = Math.cos(headingRadians); + double sin = Math.sin(headingRadians); + // Standard rotation by -heading with x = right, y = forward. + return new double[]{ + forward * cos - right * sin, + right * cos + forward * sin, + }; + } + + /** + * Mixes forward, strafe-right and clockwise rotation into wheel powers. + * + * @return {frontLeft, frontRight, backLeft, backRight}, scaled so the largest magnitude is at most 1 + */ + public static double[] mix(double forward, double right, double rotate) { + double[] p = { + forward + right + rotate, + forward - right - rotate, + forward - right + rotate, + forward + right - rotate, + }; + double max = 1.0; + for (double v : p) max = Math.max(max, Math.abs(v)); + for (int i = 0; i < p.length; i++) p[i] /= max; + return p; + } +} +``` + +- [ ] **Step 4: Run the tests** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*MecanumKinematicsTest' --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`, 7 tests passed. If `robotFacingLeftMovesRightToGoFieldForward` fails with the signs swapped, the rotation direction is wrong: the existing subsystem's `atan2`/`hypot` code is the reference behaviour, and this function must match it. + +- [ ] **Step 5: Use it in the drive subsystem** + +Replace the body of `drive(...)` in `MecanumDriveSubsystem.java` with: + +```java + public void drive(double forward, double right, double rotate, boolean fieldCentric, double headingRadians) { + if (fieldCentric) { + double[] rr = MecanumKinematics.toRobotRelative(forward, right, headingRadians); + forward = rr[0]; + right = rr[1]; + } + double[] p = MecanumKinematics.mix(forward, right, rotate); + frontLeftDrive.setPower(p[0]); + frontRightDrive.setPower(p[1]); + backLeftDrive.setPower(p[2]); + backRightDrive.setPower(p[3]); + } +``` + +Add `import org.firstinspires.ftc.teamcode.MecanumKinematics;` and remove the now-unused `import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;`. + +- [ ] **Step 6: Run tests in CI** + +In `.github/workflows/build.yml`, after the "Assemble the TeamCode debug APK" step add: + +```yaml + - name: Run the TeamCode JVM unit tests + run: ./gradlew :TeamCode:testDebugUnitTest --no-daemon --stacktrace +``` + +- [ ] **Step 7: Build and test** + +```bash +./gradlew :TeamCode:assembleDebug :TeamCode:testDebugUnitTest --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 8: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumKinematics.java TeamCode/src/test TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java .github/workflows/build.yml +git commit -m "feature: mecanum maths as a unit-tested pure function" +``` + +--- + +### Task 3: Constants, bulk reads and gamepad wrapper + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java` +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java` + +**Interfaces:** +- Produces: every name in `Constants` below, used by Tasks 4 to 8. `Robot` holds `private final GamepadEx driver` and calls `driver.readButtons()` once per loop. + +- [ ] **Step 1: Write `Constants.java`** + +```java +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** Hardware names, directions and tuning values. One home so drivers and Pedro Pathing can find them. */ +public final class Constants { + private Constants() {} + + // Robot Controller configuration names. + public static final String FRONT_LEFT_DRIVE = "front_left_drive"; + public static final String FRONT_RIGHT_DRIVE = "front_right_drive"; + public static final String BACK_LEFT_DRIVE = "back_left_drive"; + public static final String BACK_RIGHT_DRIVE = "back_right_drive"; + public static final String INTAKE = "intake"; + public static final String INDEXER = "indexer"; + public static final String SHOOTER_LEFT = "shooter_left"; + public static final String SHOOTER_RIGHT = "shooter_right"; + public static final String PINPOINT = "pinpoint"; + public static final String WEBCAM = "Webcam 1"; + + // Motor directions. Left side reversed so positive power on every drive motor goes forward. + public static final DcMotorSimple.Direction FRONT_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction FRONT_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction BACK_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE; + public static final DcMotorSimple.Direction BACK_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction INDEXER_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction SHOOTER_LEFT_DIRECTION = DcMotorSimple.Direction.FORWARD; + public static final DcMotorSimple.Direction SHOOTER_RIGHT_DIRECTION = DcMotorSimple.Direction.REVERSE; + + // Pinpoint pod offsets from the tracking point, mm. X pod: left of centre positive. + // Y pod: forward of centre positive. Measure on the robot per the goBILDA setup guide. + public static final double PINPOINT_X_OFFSET_MM = 0.0; + public static final double PINPOINT_Y_OFFSET_MM = 0.0; + + // Intake and indexer open-loop powers. + public static final double INTAKE_POWER = 1.0; + public static final double INDEXER_POWER = 1.0; + + // Shooter. 1:1 Yellow Jacket, 28 ticks per rev, 6000 RPM free speed. + public static final double SHOOTER_TICKS_PER_REV = 28.0; + public static final double SHOOTER_SETPOINT_RPM = 3500.0; // tune on the robot + public static final double SHOOTER_TOLERANCE_RPM = 100.0; + // Velocity PIDF starting point: F = 32767 / max ticks per second, P = 0.1 F, I = 0.1 P, D = 0. + public static final double SHOOTER_P = 1.17; + public static final double SHOOTER_I = 0.117; + public static final double SHOOTER_D = 0.0; + public static final double SHOOTER_F = 11.7; + + // Driver controls. + public static final double TRIGGER_THRESHOLD = 0.2; + + // Aim assist: rotation = -AIM_KP * bearingDeg, clamped. Bearing is positive to the left. + public static final double AIM_KP = 0.02; + public static final double AIM_MAX_ROTATE = 0.5; + public static final double AIM_DEADBAND_DEG = 1.0; +} +``` + +- [ ] **Step 2: Write `Alliance.java`** + +```java +package org.firstinspires.ftc.teamcode; + +public enum Alliance { RED, BLUE } +``` + +- [ ] **Step 3: Point the existing subsystems at `Constants`** + +In `MecanumDriveSubsystem.java` replace the constructor body with: + +```java + frontLeftDrive = hardwareMap.get(DcMotor.class, Constants.FRONT_LEFT_DRIVE); + frontRightDrive = hardwareMap.get(DcMotor.class, Constants.FRONT_RIGHT_DRIVE); + backLeftDrive = hardwareMap.get(DcMotor.class, Constants.BACK_LEFT_DRIVE); + backRightDrive = hardwareMap.get(DcMotor.class, Constants.BACK_RIGHT_DRIVE); + + frontLeftDrive.setDirection(Constants.FRONT_LEFT_DIRECTION); + frontRightDrive.setDirection(Constants.FRONT_RIGHT_DIRECTION); + backLeftDrive.setDirection(Constants.BACK_LEFT_DIRECTION); + backRightDrive.setDirection(Constants.BACK_RIGHT_DIRECTION); + + frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +``` + +and add `import org.firstinspires.ftc.teamcode.Constants;`. + +In `OdometrySubsystem.java` change the two lines + +```java + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); +``` +```java + pinpoint.setOffsets(0.0, 0.0, DistanceUnit.MM); +``` + +to + +```java + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, Constants.PINPOINT); +``` +```java + pinpoint.setOffsets(Constants.PINPOINT_X_OFFSET_MM, Constants.PINPOINT_Y_OFFSET_MM, DistanceUnit.MM); +``` + +add `import org.firstinspires.ftc.teamcode.Constants;`, and delete the `// TODO: measure the pods' offsets ...` comment (the note now lives on the constants). + +- [ ] **Step 4: Rewrite `Robot.java` with bulk caching and `GamepadEx`** + +Replace the whole file: + +```java +package org.firstinspires.ftc.teamcode; + +import com.arcrobotics.ftclib.command.CommandScheduler; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; + +public class Robot { + private final Telemetry telemetry; + private final GamepadEx driver; + + public final MecanumDriveSubsystem drive; + public final OdometrySubsystem odometry; + + public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) { + this.telemetry = telemetry; + this.driver = new GamepadEx(driverGamepad); + + // One bulk read per hub per loop instead of one bus transaction per motor read. + for (LynxModule hub : hardwareMap.getAll(LynxModule.class)) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + CommandScheduler.getInstance().reset(); + + drive = new MecanumDriveSubsystem(hardwareMap); + odometry = new OdometrySubsystem(hardwareMap); + } + + /** Called once when the OpMode enters INIT. */ + public void init() { + odometry.init(); + } + + /** Called repeatedly while the OpMode is running. */ + public void periodic() { + driver.readButtons(); + CommandScheduler.getInstance().run(); + + if (driver.wasJustPressed(GamepadKeys.Button.A)) { + odometry.resetHeading(); + } + + // Hold left bumper to drive robot-relative; otherwise drive field-relative. + boolean fieldCentric = !driver.isDown(GamepadKeys.Button.LEFT_BUMPER); + drive.drive(driver.getLeftY(), driver.getLeftX(), driver.getRightX(), + fieldCentric, odometry.getPose().getHeading(AngleUnit.RADIANS)); + + telemetry.addData("Pose", odometry.getPose()); + telemetry.update(); + } + + /** Called once when the OpMode stops. */ + public void stop() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().reset(); + } +} +``` + +Note `GamepadEx.getLeftY()` already negates the raw stick, so the old `-driver.left_stick_y` becomes `driver.getLeftY()`. + +- [ ] **Step 5: Build and test** + +```bash +./gradlew :TeamCode:assembleDebug :TeamCode:testDebugUnitTest --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 6: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode +git commit -m "feature: constants class, hub bulk reads and GamepadEx controls" +``` + +--- + +### Task 4: Intake and indexer subsystems + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java` +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java` + +**Interfaces:** +- Consumes: `Constants.INTAKE`, `INDEXER`, `INTAKE_DIRECTION`, `INDEXER_DIRECTION`, `INTAKE_POWER`, `INDEXER_POWER`, `TRIGGER_THRESHOLD`. +- Produces: `IntakeSubsystem.run()`, `reverse()`, `stop()`; `IndexerSubsystem.feed()`, `reverse()`, `stop()`. Task 6 adds the fire condition around `indexer.feed()`. + +- [ ] **Step 1: Write `IntakeSubsystem.java`** + +```java +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Constants; + +/** Front compliant-wheel roller. Open loop. */ +public class IntakeSubsystem extends SubsystemBase { + private final DcMotorEx motor; + + public IntakeSubsystem(HardwareMap hardwareMap) { + motor = hardwareMap.get(DcMotorEx.class, Constants.INTAKE); + motor.setDirection(Constants.INTAKE_DIRECTION); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + public void run() { + motor.setPower(Constants.INTAKE_POWER); + } + + public void reverse() { + motor.setPower(-Constants.INTAKE_POWER); + } + + public void stop() { + motor.setPower(0); + } +} +``` + +- [ ] **Step 2: Write `IndexerSubsystem.java`** + +```java +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Constants; + +/** Inclined compliant-wheel ramp that carries balls to the shooter. Brakes so held balls stay put. */ +public class IndexerSubsystem extends SubsystemBase { + private final DcMotorEx motor; + + public IndexerSubsystem(HardwareMap hardwareMap) { + motor = hardwareMap.get(DcMotorEx.class, Constants.INDEXER); + motor.setDirection(Constants.INDEXER_DIRECTION); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void feed() { + motor.setPower(Constants.INDEXER_POWER); + } + + public void reverse() { + motor.setPower(-Constants.INDEXER_POWER); + } + + public void stop() { + motor.setPower(0); + } +} +``` + +- [ ] **Step 3: Wire them into `Robot.java`** + +Add imports: + +```java +import org.firstinspires.ftc.teamcode.subsystems.IndexerSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem; +``` + +Add fields after `odometry`: + +```java + public final IntakeSubsystem intake; + public final IndexerSubsystem indexer; +``` + +Construct them after `odometry = ...`: + +```java + intake = new IntakeSubsystem(hardwareMap); + indexer = new IndexerSubsystem(hardwareMap); +``` + +In `periodic()`, after the drive call and before telemetry, add: + +```java + double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER); + double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); + if (leftTrigger > Constants.TRIGGER_THRESHOLD) { + intake.reverse(); + indexer.reverse(); + } else if (rightTrigger > Constants.TRIGGER_THRESHOLD) { + intake.run(); + indexer.feed(); + } else { + intake.stop(); + indexer.stop(); + } +``` + +- [ ] **Step 4: Build** + +```bash +./gradlew :TeamCode:assembleDebug --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 5: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode +git commit -m "feature: intake and indexer on the triggers" +``` + +--- + +### Task 5: Shooter maths, tested + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java` +- Test: `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java` + +**Interfaces:** +- Produces: `static double ShooterMath.rpmToTicksPerSecond(double rpm, double ticksPerRev)`, `static double ShooterMath.ticksPerSecondToRpm(double ticksPerSecond, double ticksPerRev)`. + +- [ ] **Step 1: Write the failing tests** + +```java +package org.firstinspires.ftc.teamcode; + +import static org.junit.Assert.assertEquals; + +import org.junit.Test; + +public class ShooterMathTest { + @Test + public void freeSpeedOfOneToOneMotorIs2800TicksPerSecond() { + assertEquals(2800.0, ShooterMath.rpmToTicksPerSecond(6000, 28), 1e-9); + } + + @Test + public void conversionsRoundTrip() { + double rpm = 3500; + double tps = ShooterMath.rpmToTicksPerSecond(rpm, 28); + assertEquals(rpm, ShooterMath.ticksPerSecondToRpm(tps, 28), 1e-9); + } +} +``` + +- [ ] **Step 2: Run and watch it fail** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*ShooterMathTest' --no-daemon +``` + +Expected: compile failure, `cannot find symbol ShooterMath`. + +- [ ] **Step 3: Implement** + +```java +package org.firstinspires.ftc.teamcode; + +/** Unit conversions for the flywheel encoders. */ +public final class ShooterMath { + private ShooterMath() {} + + public static double rpmToTicksPerSecond(double rpm, double ticksPerRev) { + return rpm * ticksPerRev / 60.0; + } + + public static double ticksPerSecondToRpm(double ticksPerSecond, double ticksPerRev) { + return ticksPerSecond * 60.0 / ticksPerRev; + } +} +``` + +- [ ] **Step 4: Run the tests** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*ShooterMathTest' --no-daemon +``` + +Expected: 2 tests passed. + +- [ ] **Step 5: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java +git commit -m "feature: shooter RPM conversions with tests" +``` + +--- + +### Task 6: Shooter subsystem, spin-up toggle and fire + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java` + +**Interfaces:** +- Consumes: `ShooterMath` (Task 5), `Constants.SHOOTER_*` (Task 3), `IndexerSubsystem.feed()` (Task 4). +- Produces: `ShooterSubsystem.spinUp()`, `idle()`, `toggle()`, `isRunning()`, `atSpeed()`, `getLeftRpm()`, `getRightRpm()`. Task 8 reads the RPM getters for telemetry. + +- [ ] **Step 1: Write `ShooterSubsystem.java`** + +```java +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Constants; +import org.firstinspires.ftc.teamcode.ShooterMath; + +/** Two flywheels, one motor each, held at a shared RPM by the hub's velocity controller. */ +public class ShooterSubsystem extends SubsystemBase { + private final DcMotorEx left; + private final DcMotorEx right; + private boolean running; + + public ShooterSubsystem(HardwareMap hardwareMap) { + left = hardwareMap.get(DcMotorEx.class, Constants.SHOOTER_LEFT); + right = hardwareMap.get(DcMotorEx.class, Constants.SHOOTER_RIGHT); + left.setDirection(Constants.SHOOTER_LEFT_DIRECTION); + right.setDirection(Constants.SHOOTER_RIGHT_DIRECTION); + for (DcMotorEx m : new DcMotorEx[]{left, right}) { + m.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + m.setVelocityPIDFCoefficients(Constants.SHOOTER_P, Constants.SHOOTER_I, Constants.SHOOTER_D, Constants.SHOOTER_F); + } + } + + public void spinUp() { + running = true; + double tps = ShooterMath.rpmToTicksPerSecond(Constants.SHOOTER_SETPOINT_RPM, Constants.SHOOTER_TICKS_PER_REV); + left.setVelocity(tps); + right.setVelocity(tps); + } + + public void idle() { + running = false; + left.setVelocity(0); + right.setVelocity(0); + } + + public void toggle() { + if (running) idle(); else spinUp(); + } + + public boolean isRunning() { + return running; + } + + /** True when both wheels are within tolerance of the setpoint. */ + public boolean atSpeed() { + return running + && Math.abs(getLeftRpm() - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM + && Math.abs(getRightRpm() - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM; + } + + public double getLeftRpm() { + return ShooterMath.ticksPerSecondToRpm(left.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + } + + public double getRightRpm() { + return ShooterMath.ticksPerSecondToRpm(right.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + } +} +``` + +- [ ] **Step 2: Wire it into `Robot.java`** + +Add import `import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem;`, field `public final ShooterSubsystem shooter;`, and construct `shooter = new ShooterSubsystem(hardwareMap);` after `indexer`. + +In `periodic()`, before the trigger block, add: + +```java + if (driver.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { + shooter.toggle(); + } + boolean fire = driver.isDown(GamepadKeys.Button.X); +``` + +Replace the trigger block with this version, which adds the fire branch: + +```java + double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER); + double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); + if (leftTrigger > Constants.TRIGGER_THRESHOLD) { + intake.reverse(); + indexer.reverse(); + } else if (rightTrigger > Constants.TRIGGER_THRESHOLD) { + intake.run(); + indexer.feed(); + } else if (fire && shooter.atSpeed()) { + // Fire: feed only while the flywheels are at speed so every shot leaves at the same velocity. + intake.stop(); + indexer.feed(); + } else { + intake.stop(); + indexer.stop(); + } +``` + +In `stop()`, before `cancelAll()`, add `shooter.idle();`. + +- [ ] **Step 3: Build** + +```bash +./gradlew :TeamCode:assembleDebug --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 4: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode +git commit -m "feature: closed-loop shooter with spin-up toggle and fire on X" +``` + +--- + +### Task 7: Vision subsystem, alliance select and aim assist + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java` + +**Interfaces:** +- Consumes: SDK v12 `AprilTagProcessor`, `AprilTagClusterDetection`, `AprilTagGameDatabase.getCurrentGameTagLibrary()`, `VisionPortal`; `Alliance` and `Constants.WEBCAM`, `AIM_*` (Task 3). +- Produces: `VisionSubsystem.setAlliance(Alliance)`, `getAlliance()`, `hasTarget()`, `getBearingDeg()`, `getTargetName()`, `stopLiveView()`, `close()`; `Robot.initLoop()`, `Robot.start()`. + +Background: the v12 BIOBUZZ library defines four clusters named `RED SCORING` (tags 30 to 33), `RED AUDIENCE` (34 to 37), `BLUE AUDIENCE` (38 to 41) and `BLUE SCORING` (42 to 45). `AprilTagClusterDetection.metadata.name` carries that name, and `ftcPose.bearing` is the horizontal angle to the cluster origin in degrees, positive to the left. The cluster origin is the centre of the Cell opening. + +- [ ] **Step 1: Write `VisionSubsystem.java`** + +```java +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.Alliance; +import org.firstinspires.ftc.teamcode.Constants; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +/** + * One webcam looking for our alliance's Hive cell. BIOBUZZ cells move, so this is for aiming only. + * Cluster names in the SDK library are "RED SCORING", "RED AUDIENCE", "BLUE AUDIENCE", "BLUE SCORING". + */ +public class VisionSubsystem extends SubsystemBase { + private final AprilTagProcessor processor; + private final VisionPortal portal; + private Alliance alliance = Alliance.RED; + private AprilTagClusterDetection target; + + public VisionSubsystem(HardwareMap hardwareMap) { + processor = new AprilTagProcessor.Builder() + .setTagLibrary(AprilTagGameDatabase.getCurrentGameTagLibrary()) + .build(); + portal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, Constants.WEBCAM)) + .addProcessor(processor) + .build(); + } + + public void setAlliance(Alliance alliance) { + this.alliance = alliance; + } + + public Alliance getAlliance() { + return alliance; + } + + /** Picks the best-seen cluster belonging to our alliance from the latest frame. */ + @Override + public void periodic() { + AprilTagClusterDetection best = null; + for (AprilTagDetection d : processor.getDetections()) { + if (!(d instanceof AprilTagClusterDetection)) continue; + AprilTagClusterDetection c = (AprilTagClusterDetection) d; + if (!c.metadata.name.startsWith(alliance.name())) continue; + if (best == null || c.percentClusterFound > best.percentClusterFound) best = c; + } + target = best; + } + + public boolean hasTarget() { + return target != null; + } + + /** Horizontal angle to the cell opening, degrees, positive to the left. Only valid when hasTarget(). */ + public double getBearingDeg() { + return target.ftcPose.bearing; + } + + public String getTargetName() { + return target == null ? "none" : target.metadata.name; + } + + /** Turn off the Driver Station preview once the match starts to save CPU. */ + public void stopLiveView() { + portal.stopLiveView(); + } + + public void close() { + portal.close(); + } +} +``` + +- [ ] **Step 2: Wire it into `Robot.java`** + +Add imports: + +```java +import org.firstinspires.ftc.teamcode.subsystems.VisionSubsystem; +``` + +Add field `public final VisionSubsystem vision;` and construct `vision = new VisionSubsystem(hardwareMap);` after `shooter`. + +Add these two methods after `init()`: + +```java + /** Called repeatedly while the OpMode sits in INIT. D-pad left = red, right = blue. */ + public void initLoop() { + driver.readButtons(); + if (driver.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) vision.setAlliance(Alliance.RED); + if (driver.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) vision.setAlliance(Alliance.BLUE); + telemetry.addData("Alliance (dpad L/R)", vision.getAlliance()); + telemetry.addData("Camera sees", vision.getTargetName()); + telemetry.update(); + } + + /** Called once when the driver presses START. */ + public void start() { + vision.stopLiveView(); + } +``` + +`initLoop()` needs the scheduler to have run `vision.periodic()` for "Camera sees" to update, so add `CommandScheduler.getInstance().run();` as its first line after `driver.readButtons()`. + +Replace the drive call in `periodic()` with aim assist: + +```java + boolean fieldCentric = !driver.isDown(GamepadKeys.Button.LEFT_BUMPER); + double rotate = driver.getRightX(); + if (driver.isDown(GamepadKeys.Button.Y) && vision.hasTarget()) { + rotate = aimRotation(vision.getBearingDeg()); + } + drive.drive(driver.getLeftY(), driver.getLeftX(), rotate, + fieldCentric, odometry.getPose().getHeading(AngleUnit.RADIANS)); +``` + +Add this private method at the bottom of the class: + +```java + /** P-controller from tag bearing (deg, +left) to clockwise rotation power. */ + private static double aimRotation(double bearingDeg) { + if (Math.abs(bearingDeg) < Constants.AIM_DEADBAND_DEG) return 0; + double rotate = -Constants.AIM_KP * bearingDeg; + return Math.max(-Constants.AIM_MAX_ROTATE, Math.min(Constants.AIM_MAX_ROTATE, rotate)); + } +``` + +In `stop()`, add `vision.close();` after `shooter.idle();`. + +- [ ] **Step 3: Call the new hooks from `Main.java`** + +Add after `init()`: + +```java + @Override + public void init_loop() { + robot.initLoop(); + } + + @Override + public void start() { + robot.start(); + } +``` + +- [ ] **Step 4: Build** + +```bash +./gradlew :TeamCode:assembleDebug --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 5: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode +git commit -m "feature: AprilTag aim assist on the alliance cell, alliance picked in init" +``` + +--- + +### Task 8: Telemetry, README table and checklist + +**Files:** +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java` +- Modify: `README.md` + +**Interfaces:** +- Consumes: `ShooterSubsystem.getLeftRpm()`, `getRightRpm()`, `atSpeed()`, `isRunning()`; `VisionSubsystem.hasTarget()`, `getBearingDeg()`, `getAlliance()`; `OdometrySubsystem.getPose()`. + +- [ ] **Step 1: Add the voltage sensor and telemetry to `Robot.java`** + +Add import `import com.qualcomm.robotcore.hardware.VoltageSensor;` and field `private final VoltageSensor battery;`. In the constructor, after the bulk-caching loop: + +```java + battery = hardwareMap.voltageSensor.iterator().next(); +``` + +Replace the telemetry lines at the end of `periodic()` with: + +```java + telemetry.addData("Alliance", vision.getAlliance()); + telemetry.addData("Pose", odometry.getPose()); + telemetry.addData("Shooter", "%s target %.0f L %.0f R %.0f %s", + shooter.isRunning() ? "ON" : "off", Constants.SHOOTER_SETPOINT_RPM, + shooter.getLeftRpm(), shooter.getRightRpm(), shooter.atSpeed() ? "AT SPEED" : ""); + telemetry.addData("Tag bearing", vision.hasTarget() ? String.format("%.1f deg", vision.getBearingDeg()) : "no target"); + telemetry.addData("Battery", "%.1f V", battery.getVoltage()); + telemetry.update(); +``` + +- [ ] **Step 2: Add the configuration table and checklist to the README** + +Append to `README.md`: + +```markdown +## Control Hub configuration + +The Robot Controller configuration must use these names. The PR template asks +you to keep this table current when you add or move hardware. + +| Config name | Type | Hub / port | Mechanism | +| --- | --- | --- | --- | +| `front_left_drive` | goBILDA 5202/3/4 series | | Drive | +| `front_right_drive` | goBILDA 5202/3/4 series | | Drive | +| `back_left_drive` | goBILDA 5202/3/4 series | | Drive | +| `back_right_drive` | goBILDA 5202/3/4 series | | Drive | +| `intake` | goBILDA 5202/3/4 series | | Intake | +| `indexer` | goBILDA 5202/3/4 series | | Indexer | +| `shooter_left` | goBILDA 5202/3/4 series | | Shooter | +| `shooter_right` | goBILDA 5202/3/4 series | | Shooter | +| `pinpoint` | goBILDA Pinpoint Odometry Computer (I2C) | | Odometry | +| `Webcam 1` | Webcam | USB | Vision | + +Fill in the hub and port column when the robot is wired. + +## Driver controls + +One driver on gamepad 1. Pick the alliance during INIT with D-pad left (red) +or right (blue). + +| Input | Action | +| --- | --- | +| Left stick | Translate, field-centric | +| Right stick X | Rotate | +| Left bumper (hold) | Robot-centric translate | +| A | Zero heading | +| Right trigger (hold) | Intake and indexer run | +| Left trigger (hold) | Intake and indexer reverse | +| Right bumper | Shooter on / off | +| X (hold) | Fire (indexer feeds once the shooter is at speed) | +| Y (hold) | Aim at our Hive cell | + +## Testing on the robot + +Run through these after any change to the matching subsystem and record the +result in the PR. + +1. Drive: each motor spins forward on positive power, the robot drives + straight, strafes right on stick right, and field-centric holds direction + after a spin. +2. Odometry: Pinpoint LED green. X grows driving forward, Y grows driving left, + heading grows turning anticlockwise. Spinning in place moves X/Y under + 100 mm. Returning to the start reads under 10 mm. +3. Intake and indexer: run, reverse, stop. The indexer holds a ball when + stopped. +4. Shooter: reaches the setpoint, telemetry RPM within tolerance, the AT SPEED + flag appears, and a Pollen lands in the Cell from the practice spot. +5. Vision: telemetry shows a bearing with a Cell tag in view, and holding Y + turns the robot toward it and settles. + +Tuning values (shooter RPM and PIDF, aim gain, Pinpoint pod offsets) live in +`TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java`. +``` + +- [ ] **Step 3: Build and test** + +```bash +./gradlew :TeamCode:assembleDebug :TeamCode:testDebugUnitTest --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 4: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java README.md +git commit -m "docs: driver telemetry, config table, controls and on-robot checklist" +``` + +--- + +## After the plan + +Everything above compiles and passes the JVM tests without a robot. The on-robot checklist in the README is the acceptance test. Expect to change `SHOOTER_SETPOINT_RPM`, the `SHOOTER_*` PIDF values, `AIM_KP` and the two `PINPOINT_*_OFFSET_MM` values on the first day with the robot; each is one line in `Constants.java`. From 78e4384364886c5ac55906007d8e92c4f04206f6 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:25:47 +0800 Subject: [PATCH 3/7] chore: move to FTC SDK v12.0, the BIOBUZZ season release --- .../src/main/AndroidManifest.xml | 2 +- .../external/samples/ConceptAprilTag.java | 27 +++-- .../external/samples/ConceptAprilTagEasy.java | 50 ++++---- .../samples/ConceptAprilTagLocalization.java | 22 ++-- .../ConceptAprilTagSwitchableCameras.java | 23 +++- .../samples/RobotAutoDriveToAprilTagOmni.java | 111 ++++++++++------- .../samples/RobotAutoDriveToAprilTagTank.java | 112 +++++++++++------- README.md | 2 +- TeamCode/build.gradle | 2 +- build.dependencies.gradle | 16 +-- 10 files changed, 225 insertions(+), 142 deletions(-) diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 8ff3b6e..02da1ca 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -2,7 +2,7 @@ + android:versionName="12.0"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java index 44abd97..f8d754c 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java @@ -38,8 +38,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.internal.usb.UsbConstants; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -150,12 +152,12 @@ private void initAprilTag() { aprilTag = new AprilTagProcessor.Builder() // The following default settings are available to un-comment and edit as needed. - //.setDrawAxes(false) - //.setDrawCubeProjection(false) + //.setDrawAxes(true) // Changed in V12.0 //.setDrawTagOutline(true) //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + //.setDrawCubeProjection(false) // == CAMERA CALIBRATION == // If you do not manually specify calibration parameters, the SDK will attempt @@ -220,14 +222,25 @@ private void telemetryAprilTag() { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name)); + telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound)); telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java index 76ace3c..085f133 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -36,8 +36,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCompatibilityManager; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -45,6 +47,8 @@ * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using * the easy way. * + * Note: See ConceptAprilTag.java for how to add a camera compatibility quirk + * * For an introduction to AprilTags, see the FTC-DOCS link below: * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html * @@ -78,33 +82,10 @@ public class ConceptAprilTagEasy extends LinearOpMode { */ private VisionPortal visionPortal; - // To find the VID/PID for a camera: - // - // Linux: open a terminal, run "lsusb", locate the line for your camera, - // and find the section that resembles "ID 1d6b:0002"; this is VID:PID - // - // OSX: open a terminal, run "system_profiler SPUSBDataType", locate the - // section for your camera, and find the "Product ID:" and "Vendor ID:" - // listings in the output - // - // Windows: open a PowerShell, run: - // Get-PnpDevice -PresentOnly | Where-Object { $_.InstanceId -like 'USB*' } | Select-Object FriendlyName, InstanceId - // and locate the line for your camera. The VID and PID is listed directly in the line. - static final int VENDOR_ID_SUNPLUS_INNOVATION_TECHNOLOGY = 0x1BCF; - static final int PRODUCT_ID_ARDUCAM_OV5648 = 0x284C; - @Override public void runOpMode() { - // Demonstrate how to add a camera compatibility quirk - // these can sometimes be needed if a camera behaves poorly. - // Quirks have no effect unless the camera you are using matches the specified VID/PID - CameraCompatibilityManager.getInstance() - .addQuirk( - VENDOR_ID_SUNPLUS_INNOVATION_TECHNOLOGY, - PRODUCT_ID_ARDUCAM_OV5648, - CameraCompatibilityManager.Quirk.AVOID_LIB_USB_RESET_DEVICE); - + // See ConceptAprilTag.java for how to add a camera compatibility quirk initAprilTag(); // Wait for the DS start button to be touched. @@ -165,14 +146,25 @@ private void telemetryAprilTag() { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name)); + telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound)); telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java index a14b971..53bed1a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -43,6 +43,7 @@ import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -177,7 +178,7 @@ private void initAprilTag() { aprilTag = new AprilTagProcessor.Builder() // The following default settings are available to un-comment and edit as needed. - //.setDrawAxes(false) + //.setDrawAxes(true) // changed in V12.0 //.setDrawCubeProjection(false) //.setDrawTagOutline(true) //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) @@ -247,22 +248,23 @@ private void telemetryAprilTag() { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); - // Only use tags that don't have Obelisk in them - if (!detection.metadata.name.contains("Obelisk")) { - telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("Robot XYZ %6.1f %6.1f %6.1f (inch)", detection.robotPose.getPosition().x, detection.robotPose.getPosition().y, detection.robotPose.getPosition().z)); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + telemetry.addLine(String.format("Robot PRY %6.1f %6.1f %6.1f (deg)", detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); } - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java index 02e83d3..06068b3 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java @@ -37,8 +37,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.VisionPortal.CameraState; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -153,14 +155,25 @@ private void telemetryAprilTag() { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name)); + telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound)); telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java index 4b777e2..af58d40 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java @@ -39,25 +39,32 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; import java.util.concurrent.TimeUnit; /* - * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag or AprilTag Cluster + * A "Cluster" is a group of Apriltags that share a common origin, and are identified by name. * The code assumes a Holonomic (Mecanum or X Drive) Robot. * * For an introduction to AprilTags, see the ftc-docs link below: * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html * - * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * When an AprilTag/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera. * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. * https://ftc-docs.firstinspires.org/apriltag-detection-values * - * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and - * driving towards the tag to achieve the desired distance. + * For a single tag, the "Drive Target" is the center of the Tag. For a cluster of tags, the "Drive Target" will be the + * (0,0,0) ORIGIN of the cluster, which may have been positioned somewhere other than the center of the cluster in order + * to help to locate a game objective. + * + * The driving goal is to rotate to keep the Target centered in the camera, while strafing to be directly in front of the target, + * and driving towards the target to achieve the desired distance. * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. * @@ -73,9 +80,9 @@ * Release the Left Bumper to return to manual driving mode. * * Under "Drive To Target" mode, the robot has three goals: - * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) - * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot) - * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * 1) Turn the robot to always keep the Target centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Strafe the robot towards the centerline of the Target, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot) + * 3) Drive towards the Target to get to the desired distance. (Use TargetRange to drive the robot forward/backward) * * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants. @@ -90,7 +97,7 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode { // Adjust these numbers to suit your robot. - final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + final double DESIRED_DISTANCE = 30.0; // this is how close the camera should get to the target (inches) // Set the GAIN constants to control the relationship between the measured position error, and how much power is // applied to the drive motors to correct the error. @@ -99,24 +106,31 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". e.g. Ramp up to 37% power at a 25 degree Yaw error. (0.375 / 25.0) final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) - final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) - final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot) - final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) + final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot) + final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) private DcMotor frontLeftDrive = null; // Used to control the left front drive wheel - private DcMotor frontRightDrive = null; // Used to control the right front drive wheel - private DcMotor backLeftDrive = null; // Used to control the left back drive wheel + private DcMotor frontRightDrive = null; // Used to control the right front drive wheel + private DcMotor backLeftDrive = null; // Used to control the left back drive wheel private DcMotor backRightDrive = null; // Used to control the right back drive wheel - private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera - private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private final int DESIRED_TAG_ID = -1; // The tag you want to approach, or set to -1 for ANY tag. + private final String DESIRED_CLUSTER_NAME = null; // The cluster name you want to approach, or set null for ANY cluster. + private VisionPortal visionPortal; // Used to manage the video source. private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. - private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + private boolean targetFound = false; // Set to true when an AprilTag/Cluster target is detected + private String targetName = "none"; + private int targetID = 0; + private double targetRange = 0; + private double targetBearing = 0; + private double targetYaw = 0; @Override public void runOpMode() { - boolean targetFound = false; // Set to true when an AprilTag target is detected double drive = 0; // Desired forward power/speed (-1 to +1) double strafe = 0; // Desired strafe power/speed (-1 to +1) double turn = 0; // Desired turning power/speed (-1 to +1) @@ -152,36 +166,57 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode while (opModeIsActive()) { targetFound = false; - desiredTag = null; // Step through the list of detected tags and look for a matching tag List currentDetections = aprilTag.getDetections(); for (AprilTagDetection detection : currentDetections) { - // Look to see if we have size info on this tag. - if (detection.metadata != null) { - // Check to see if we want to track towards this tag. - if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { + + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection; + + // Look to see if we have size info on this tag. + if (singleDetection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (singleDetection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetName = singleDetection.metadata.name; + targetID = singleDetection.id; + targetRange = singleDetection.ftcPose.range; + targetBearing = singleDetection.ftcPose.bearing; + targetYaw = singleDetection.ftcPose.yaw; + targetFound = true; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", singleDetection.id); + } + } else { + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", singleDetection.id); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + + if (DESIRED_CLUSTER_NAME == null || clusterDet.metadata.shortName.equals(DESIRED_CLUSTER_NAME) ) { // Yes, we want to use this tag. + targetName = clusterDet.metadata.shortName; + targetID = -1; + targetRange = clusterDet.ftcPose.range; + targetBearing = clusterDet.ftcPose.bearing; + targetYaw = clusterDet.ftcPose.yaw; targetFound = true; - desiredTag = detection; break; // don't look any further. - } else { - // This tag is in the library, but we do not want to track it right now. - telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); } - } else { - // This tag is NOT in the library, so we don't have enough information to track to it. - telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); } } // Tell the driver what we see, and what to do. if (targetFound) { telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); - telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); - telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); - telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); - telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw); + telemetry.addData("Found", "ID %d (%s)", targetID, targetName); + telemetry.addData("Range", "%5.1f inches", targetRange); + telemetry.addData("Bearing","%3.0f degrees", targetBearing); + telemetry.addData("Yaw","%3.0f degrees", targetYaw); } else { telemetry.addData("\n>","Drive using joysticks to find valid target\n"); } @@ -190,9 +225,9 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode if (gamepad1.left_bumper && targetFound) { // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically. - double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); - double headingError = desiredTag.ftcPose.bearing; - double yawError = desiredTag.ftcPose.yaw; + double rangeError = targetRange - DESIRED_DISTANCE; + double headingError = targetBearing; + double yawError = targetYaw; // Use the speed and turn "gains" to calculate how we want the robot to move. drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); @@ -201,7 +236,6 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn); } else { - // drive using manual POV Joystick mode. Slow things down to make the robot more controlable. drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. strafe = -gamepad1.left_stick_x / 2.0; // Reduce strafe rate to 50%. @@ -218,11 +252,8 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode /** * Move robot according to desired axes motions - *

* Positive X is forward - *

* Positive Y is strafe left - *

* Positive Yaw is counter-clockwise */ public void moveRobot(double x, double y, double yaw) { diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java index ba3eb4f..09cad83 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java @@ -39,44 +39,50 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; import java.util.concurrent.TimeUnit; /* - * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. - * The code assumes a basic two-wheel (Tank) Robot Drivetrain + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag or AprilTag Cluster + * A "Cluster" is a group of Apriltags that share a common origin, and are identified by name. + * The code assumes a basic two-motor Tank (differential) drive robot. * * For an introduction to AprilTags, see the ftc-docs link below: * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html * - * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * When an AprilTag/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera. * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. * https://ftc-docs.firstinspires.org/apriltag-detection-values * - * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance. + * For a single tag, the "Drive Target" is the center of the Tag. For a cluster of tags, the "Drive Target" will be the + * (0,0,0) ORIGIN of the cluster, which may have been positioned somewhere other than the center of the cluster in order + * to help to locate a game objective. + * + * The driving goal is to rotate to keep the Target centered in the camera, while driving towards the target to achieve the desired distance. * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) - * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. + * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. * - * The code assumes a Robot Configuration with motors named left_drive and right_drive. - * The motor directions must be set so a positive power goes forward on both wheels; - * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default + * The code assumes a Robot Configuration with motors named: left_drive and right_drive. + * The motor directions must be set so a positive power goes forward on all wheels. + * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default, * so you should choose to approach a valid tag ID. * - * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot. - * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel). - * + * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot. * Manually drive the robot until it displays Target data on the Driver Station. + * * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode. * Release the Left Bumper to return to manual driving mode. * - * Under "Drive To Target" mode, the robot has two goals: - * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) - * 2) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * Under "Drive To Target" mode, the robot has two goals: + * 1) Turn the robot to always keep the Target centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Drive towards the Target to get to the desired distance. (Use TargetRange to drive the robot forward/backward) * - * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. + * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants. * * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder. @@ -89,7 +95,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode { // Adjust these numbers to suit your robot. - final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + final double DESIRED_DISTANCE = 30.0; // this is how close the camera should get to the target (inches) // Set the GAIN constants to control the relationship between the measured position error, and how much power is // applied to the drive motors to correct the error. @@ -103,15 +109,22 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode private DcMotor leftDrive = null; // Used to control the left drive wheel private DcMotor rightDrive = null; // Used to control the right drive wheel - private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera - private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private final int DESIRED_TAG_ID = -1; // The tag you want to approach, or set to -1 for ANY tag. + private final String DESIRED_CLUSTER_NAME = null; // The cluster name you want to approach, or set null for ANY cluster. + private VisionPortal visionPortal; // Used to manage the video source. private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. - private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + private boolean targetFound = false; // Set to true when an AprilTag/Cluster target is detected + private String targetName = "none"; + private int targetID = 0; + private double targetRange = 0; + private double targetBearing = 0; + private double targetYaw = 0; @Override public void runOpMode() { - boolean targetFound = false; // Set to true when an AprilTag target is detected double drive = 0; // Desired forward power/speed (-1 to +1) +ve is forward double turn = 0; // Desired turning power/speed (-1 to +1) +ve is CounterClockwise @@ -142,35 +155,57 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode while (opModeIsActive()) { targetFound = false; - desiredTag = null; // Step through the list of detected tags and look for a matching tag List currentDetections = aprilTag.getDetections(); for (AprilTagDetection detection : currentDetections) { - // Look to see if we have size info on this tag. - if (detection.metadata != null) { - // Check to see if we want to track towards this tag. - if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { + + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection; + + // Look to see if we have size info on this tag. + if (singleDetection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (singleDetection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetName = singleDetection.metadata.name; + targetID = singleDetection.id; + targetRange = singleDetection.ftcPose.range; + targetBearing = singleDetection.ftcPose.bearing; + targetYaw = singleDetection.ftcPose.yaw; + targetFound = true; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", singleDetection.id); + } + } else { + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", singleDetection.id); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + + if (DESIRED_CLUSTER_NAME == null || clusterDet.metadata.shortName.equals(DESIRED_CLUSTER_NAME) ) { // Yes, we want to use this tag. + targetName = clusterDet.metadata.shortName; + targetID = -1; + targetRange = clusterDet.ftcPose.range; + targetBearing = clusterDet.ftcPose.bearing; + targetYaw = clusterDet.ftcPose.yaw; targetFound = true; - desiredTag = detection; break; // don't look any further. - } else { - // This tag is in the library, but we do not want to track it right now. - telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); } - } else { - // This tag is NOT in the library, so we don't have enough information to track to it. - telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); } } // Tell the driver what we see, and what to do. if (targetFound) { telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); - telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); - telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); - telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); + telemetry.addData("Found", "ID %d (%s)", targetID, targetName); + telemetry.addData("Range", "%5.1f inches", targetRange); + telemetry.addData("Bearing","%3.0f degrees", targetBearing); + telemetry.addData("Yaw","%3.0f degrees", targetYaw); } else { telemetry.addData("\n>","Drive using joysticks to find valid target\n"); } @@ -179,8 +214,8 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode if (gamepad1.left_bumper && targetFound) { // Determine heading and range error so we can use them to control the robot automatically. - double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); - double headingError = desiredTag.ftcPose.bearing; + double rangeError = targetRange - DESIRED_DISTANCE; + double headingError = targetBearing; // Use the speed and turn "gains" to calculate how we want the robot to move. Clip it to the maximum drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); @@ -188,7 +223,6 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn); } else { - // drive using manual POV Joystick mode. drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. turn = -gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%. @@ -204,9 +238,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode /** * Move robot according to desired axes motions - *

* Positive X is forward - *

* Positive Yaw is counter-clockwise */ public void moveRobot(double x, double yaw) { diff --git a/README.md b/README.md index faf7577..6fd496f 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ our own code in `TeamCode`. - **JDK 17.** - **Android SDK packages** (listed below) -- **FIRST Tech Challenge SDK v11.2.1** (already in this repository) +- **FIRST Tech Challenge SDK v12.0** (already in this repository, the BIOBUZZ season release) The required SDK packages: diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 267cfb3..da862ab 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -27,5 +27,5 @@ android { dependencies { implementation project(':FtcRobotController') implementation 'org.ftclib.ftclib:core:2.1.1' - implementation 'org.ftclib.ftclib:vision:2.1.0' + testImplementation 'junit:junit:4.13.2' } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 8989a4c..390cc64 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -4,14 +4,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:11.2.1' - implementation 'org.firstinspires.ftc:Blocks:11.2.1' - implementation 'org.firstinspires.ftc:RobotCore:11.2.1' - implementation 'org.firstinspires.ftc:RobotServer:11.2.1' - implementation 'org.firstinspires.ftc:OnBotJava:11.2.1' - implementation 'org.firstinspires.ftc:Hardware:11.2.1' - implementation 'org.firstinspires.ftc:FtcCommon:11.2.1' - implementation 'org.firstinspires.ftc:Vision:11.2.1' + implementation 'org.firstinspires.ftc:Inspection:12.0.0' + implementation 'org.firstinspires.ftc:Blocks:12.0.0' + implementation 'org.firstinspires.ftc:RobotCore:12.0.0' + implementation 'org.firstinspires.ftc:RobotServer:12.0.0' + implementation 'org.firstinspires.ftc:OnBotJava:12.0.0' + implementation 'org.firstinspires.ftc:Hardware:12.0.0' + implementation 'org.firstinspires.ftc:FtcCommon:12.0.0' + implementation 'org.firstinspires.ftc:Vision:12.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' } From c9157975eae75b1dd4a1f0b894e1f5099fec6f68 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:25:52 +0800 Subject: [PATCH 4/7] docs: design spec for shoot on the move --- .../2026-09-20-shoot-on-the-move-design.md | 260 ++++++++++++++++++ 1 file changed, 260 insertions(+) create mode 100644 docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md diff --git a/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md b/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md new file mode 100644 index 0000000..d428189 --- /dev/null +++ b/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md @@ -0,0 +1,260 @@ +# Shoot on the move, design + +Lebob Robotics, FTC 29550 · BIOBUZZ 2026/27 · v1.0, 20 Sep 2026 + +## Purpose + +This document describes how the robot scores Pollen and Nectar into the Cell +while driving. It follows the method FRC 4414 used for REBUILT: work out every +shot that scores ahead of time on a laptop, keep the one with the most room for +error, and give the robot a table to look up at match time. The robot handles +the sideways part of its own motion live by turning toward a lead point. + +It builds on the teleop design (`2026-09-20-biobuzz-teleop-design.md`), which +listed distance-based shooter speed as out of scope. That is in scope here. + +## What we have to work with + +The shooter is two flywheels at a fixed launch angle. There is no turret and no +hood. The two things the code can change are flywheel speed and where the robot +is pointing, and a mecanum drive can turn while it translates, so the drivetrain +is the turret. + +This shapes the maths. 4414 had a hood, so their table could trade launch angle +against speed. Ours cannot. Whether a shot exists at all depends on how fast the +robot is closing on or backing away from the Cell, because the robot's radial +speed adds to the ball's and a fixed angle only lands within a narrow speed +band. The table therefore has two inputs, distance and radial velocity, and the +robot must be told when no shot exists. + +Sensors: goBILDA Pinpoint for field pose and field-frame velocity +(`getVelX`, `getVelY`, `getHeadingVelocity` are in the SDK driver), one webcam +seeing the AprilTag cluster on the underside of the Cell. + +## How the FRC teams do it + +- **4414 (REBUILT, tech binder):** for each (distance, radial velocity) pair, + simulate every hood angle and flywheel speed, keep those that score, pick the + middle of the valid band, fit a polynomial for fast lookup. Tangential + velocity is corrected live by rotating the turret. Tilt compensation covers + the FRC bump; the FTC field is flat so we skip it. +- **6328 (REBUILT, public code):** interpolated maps of distance to hood, speed + and time of flight, then a loop that predicts where the launcher will be after + the time of flight and aims from there. No physics at runtime. This is the + alternative to putting velocity into the table. We use the table instead + because a fixed hood makes the radial term change whether a shot exists, and a + lookahead loop cannot express that. +- **BIOBUZZ simulator (community, FTC Java):** subtracts robot velocity from the + wanted ball velocity and re-solves azimuth, elevation and speed. Its useful + result for us is a worked case showing a fixed hood cannot match both the + horizontal and vertical components, so closing at 0.4 m/s from 40 in leaves + the ball short of the Cell. Its POLLEN and NECTAR constants seed ours. + +## Architecture + +Three parts, kept apart so each can be checked on its own. + +1. **Shot solver and visualiser**, one HTML file run in a browser on a laptop. + It holds the physics, sweeps the inputs, draws the results and exports the + Java table. Nothing else in the repo has a copy of the ballistics. +2. **`ShotTable.java`**, a generated constants file committed to the repo, with + the settings that generated it in a header comment. +3. **Robot code**: a pure `ShotSolver` that reads the table, a `TargetTracker` + that holds the Cell position in field coordinates, and changes to `Robot`, + `ShooterSubsystem` and the drive so aiming and firing use them. + +### Offline solver and visualiser + +`tools/shots/index.html`, vanilla JavaScript with Plotly from its CDN for the +plots. Opened straight from the file system, no server, no build step. A +solver in the browser is chosen over Python because the page needs the physics +anyway to draw trajectories, and one copy of the maths is better than two that +drift. + +**Inputs**, editable in a form on the page, with the measured values from the +characterisation session (below) as defaults: + +- Ball: diameter, mass, drag coefficient, for Pollen and Nectar (toggle). + Start values from the AndyMark spec (2.80 in, 24.9 g; 3.62 in, 41.3 g) and a + drag coefficient of 0.45, which is a guess until calibrated. +- Shooter: launch angle, exit height above the tiles, exit speed per RPM + (linear, one constant), shooter offset forward of the robot centre, flywheel + speed tolerance. +- Cell: near lip height (53.5 in from the teleop spec), opening 20 in wide by + 14 in tall, opening tilt 30° from horizontal with the far edge higher than + the near lip, ball clearance margin at each edge. These are read from + Competition Manual Figure 9-10 and the field CAD before the first table is + exported, and the page shows them on the plot so an error is visible. +- Sweep ranges: distance 0.6 to 3.0 m in 0.1 m steps, radial velocity −1.0 to + +1.0 m/s in 0.1 m/s steps (positive is closing), RPM 1500 to 5500 in 25 RPM + steps. + +**Physics**: two-dimensional flight in the vertical plane through the Cell +centre, gravity plus quadratic drag, fixed-step integration at 2 ms. The ball +leaves with the exit velocity from the flywheel plus the robot's radial +velocity added horizontally. A shot scores when the path crosses the opening +segment travelling downward, clears the near lip by the ball radius plus +margin, and lands inside the far edge by the same margin. Spin, Magnus lift and +bounce out of the Cell are not modelled. + +**Selection**: for each (distance, radial velocity) the valid RPMs form a band. +The table stores the band centre, the band width, and time of flight at the +centre. A cell is marked invalid when the band is narrower than twice the +flywheel tolerance. This is 4414's "most robust to errors" rule with speed as +the only knob. No polynomial fit: the grid is 25 by 21 and the hub interpolates +it directly. + +**Visualiser**, three panels matching the 4414 binder image: + +- Trajectory fan for the selected distance and radial velocity. Every RPM in the + sweep is drawn, green if it scores, red if it misses, the chosen shot in blue. + The Cell opening and near lip are drawn to scale, with an arrow for the robot + velocity. +- Valid RPM band against radial velocity at the selected distance, with the + chosen centre line. This is where the fixed-hood limit shows: the band + closes at some closing speed and that is the speed the driver cannot exceed. +- Heatmap of chosen RPM over distance and radial velocity, invalid cells grey, + with a toggle to show band width instead. + +Two sliders (distance, radial velocity), a ball toggle, and a readout of RPM, +band width, time of flight and lateral tolerance at that distance. + +**Export**: a button downloads `ShotTable.java` with the grid as `double[][]` +arrays plus the axis values, and the input form as a comment block at the top. +A second button downloads a CSV of the same for spreadsheets. The Java file is +committed; the page is the source of truth for regenerating it. + +**Self-check on load**: with drag set to zero the integrator is compared with +the closed-form parabola at three points and must agree within 1 mm, and the +Pollen table at zero velocity must have RPM increasing with distance. A failure +shows a red banner and disables export. + +### `ShotSolver` (robot, pure Java, unit tested) + +Static-free plain class constructed with the table and the shooter constants. +One method, `solve(pose, velocity, target)`, returns a small result object: +`rpm`, `headingRad`, `valid`, and for telemetry `distance`, `radialVel`, +`tangentialVel`, `timeOfFlight`, `bandWidth`. + +Steps: + +1. Advance the pose by velocity times the feed delay (time from indexer feed + command to ball exit, measured on the robot). This is the only lookahead + needed, because robot velocity during flight is already inside the table. +2. Shooter position is the advanced pose plus the shooter offset rotated by + heading. Distance and bearing are measured from there to the target. +3. Split the field velocity into radial (along the bearing, positive closing) + and tangential components. +4. Bilinear interpolation of RPM, band width and time of flight at (distance, + radial velocity). Outside the grid or inside an invalid cell gives + `valid = false`. +5. Heading lead: the horizontal exit speed is the RPM times the exit speed + constant times cos(launch angle). The robot turns away from its tangential + motion by asin(tangential / horizontal exit speed) so the ball's horizontal + velocity, exit plus robot, points along the bearing. Radial exit speed + changes by the cosine of that lead, under 3 percent below 15°, and is + ignored. + +### `TargetTracker` (robot) + +Holds the up-facing Cell's opening centre in field coordinates. On each cluster +detection from `VisionSubsystem` it takes the robot pose at the frame's +timestamp from a ring buffer of the last 50 Pinpoint poses, adds the cluster's +robot-relative position, and stores the result with the time. Between +detections the Pinpoint carries the aim. The stored point expires after a +configurable age (start at 5 s) so a robot that has not seen a tag for a while +is told rather than left guessing. The Cell moves when the Hive tips, so no +field constant is used. + +The camera's position and yaw on the robot go into `Constants` once the mount +is in the CAD. + +### Changes to existing subsystems + +- `ShooterSubsystem`: `setTargetRpm(double)` replaces the single setpoint. + `atSpeed()` compares against the current target. A D-pad up/down trim adds + or subtracts 50 RPM to every target for the rest of the run, shown on + telemetry, for when the table is slightly off on the day. +- `OdometrySubsystem`: exposes field velocity from the Pinpoint and keeps the + timestamped pose ring buffer. +- `MecanumDriveSubsystem`: no change. `Robot` supplies the rotation input. +- `Robot`: while Y is held the rotation input comes from a proportional + heading controller on the solver's heading, the left stick still translates, + and the shooter runs at the solver's RPM. Fire (X) feeds only when the solver + is valid, the shooter is at speed, heading error is inside the tolerance + set by the Cell width at that distance, and the target is not expired. When + the solver is invalid the shooter idles at the stationary RPM for the current + distance and the Driver Station shows "NO SHOT: closing too fast" or + "NO SHOT: out of range". +- `Constants`: feed delay, shooter offset, launch angle, exit speed constant, + heading gains, target expiry, camera transform. + +### Logging + +Each loop while aiming, one CSV row to `/sdcard/FIRST/shots/.csv`: time, pose, velocity, distance, radial and tangential velocity, +table RPM, measured RPM per wheel, heading error, valid flag, fired flag. Pulled +off the hub over ADB after practice. The file is the tuning tool since Wi-Fi +dashboards are banned at events. + +## Measurements needed before the first table + +Done once on the robot, recorded in `tools/shots/measurements.md`: + +1. Launch angle, from CAD and checked with a protractor. +2. Exit height above the tiles. +3. Exit speed per RPM: fire Pollen at three RPMs from a fixed spot, slow-motion + video against a metre rule, fit a line through the origin. +4. Drag coefficient: compare the landing distance of those shots to the page's + prediction and adjust until they agree. Repeat with Nectar. +5. Feed delay: video the indexer command LED and the ball exit, or count loops + from the fire command to the shooter current spike. +6. Time of flight at two distances, to check the table's TOF column. + +## Testing + +**JVM unit tests** (`ShotSolverTest`, `ShotTableTest`): + +- Interpolation returns grid values at grid points and the midpoint between + neighbours. +- Stationary robot: heading equals the plain bearing and RPM equals the zero + velocity column. +- Strafing left past the Cell: heading is to the right of the bearing by + asin(v / horizontal exit speed). +- Closing at 0.5 m/s: RPM is lower than stationary at the same distance. +- Past the band edge or outside the grid: `valid` is false. +- Feed delay moves the distance by velocity times delay. +- `TargetTracker` places the target from a pose in the buffer, not the current + pose, and reports expiry. + +**Page self-check**, described above, runs on every load. + +**On-robot ladder**, each rung recorded in the PR before the next starts: + +1. Stationary at 1.0, 1.5, 2.0 and 2.5 m: at least 8 of 10 Pollen in the Cell. + Adjust exit speed constant or drag until the table matches, regenerate. +2. Strafing across the Cell at a steady speed with heading lead only: 7 of 10. +3. Driving toward and away at a steady speed: 7 of 10, and the "NO SHOT" + message appears at the speed the band chart predicts. +4. Free driving by the driver: log and video, count hits, fix what the log + shows. + +## Out of scope + +Acceleration compensation, spin and Magnus lift, bounce-out modelling, tilt +compensation, polynomial fitting, an on-robot tuning interface, Nectar-specific +runtime tables (the runtime uses the Pollen table until the intake can tell the +two apart), and any change to the shooter hardware. If the band chart shows the +usable closing speed is too low to be worth having, the answer is an adjustable +hood and that is a hardware conversation. + +## Open items + +- Cell opening geometry from Figure 9-10 and the CAD: tilt, and whether the + 14 in "tall" is along the tilted face or vertical. +- Camera mount position and yaw. +- Whether the camera keeps the tag in view while the robot turns to lead a shot + at high tangential speed. The tracker's expiry handles short losses; long + losses mean a wider lens or a second camera, which is out of scope here. +- Pinpoint velocity noise. If the logged radial velocity is noisy enough to + flicker the valid flag, a short moving average goes in front of the solver. From 3c394e4041f46e659d4711d612e7db1f0b318afc Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:26:14 +0800 Subject: [PATCH 5/7] docs: spec wording --- .../specs/2026-09-20-shoot-on-the-move-design.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md b/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md index d428189..0efc2a6 100644 --- a/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md +++ b/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md @@ -66,7 +66,8 @@ Three parts, kept apart so each can be checked on its own. ### Offline solver and visualiser `tools/shots/index.html`, vanilla JavaScript with Plotly from its CDN for the -plots. Opened straight from the file system, no server, no build step. A +plots (so it needs internet the first time; the browser caches it after). +Opened straight from the file system, no server, no build step. A solver in the browser is chosen over Python because the page needs the physics anyway to draw trajectories, and one copy of the maths is better than two that drift. @@ -131,7 +132,8 @@ shows a red banner and disables export. ### `ShotSolver` (robot, pure Java, unit tested) -Static-free plain class constructed with the table and the shooter constants. +A plain class with no static state, constructed with the table and the shooter +constants. One method, `solve(pose, velocity, target)`, returns a small result object: `rpm`, `headingRad`, `valid`, and for telemetry `distance`, `radialVel`, `tangentialVel`, `timeOfFlight`, `bandWidth`. From 19b54865f2a4caf9ddb0ae10ac695398aa45825f Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:27:17 +0800 Subject: [PATCH 6/7] chore: ignore .worktrees --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 8f3cb15..0d79261 100644 --- a/.gitignore +++ b/.gitignore @@ -80,4 +80,6 @@ lint/intermediates/ lint/generated/ lint/outputs/ lint/tmp/ -# lint/reports/ \ No newline at end of file +# lint/reports/ +# Linked git worktrees for parallel sessions +.worktrees/ From caa0f9afc1842fc6ec90e04243801a730c9ff4aa Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:40:47 +0800 Subject: [PATCH 7/7] docs: implementation plan for shoot on the move, spec notes band-width finding --- .../plans/2026-09-20-shoot-on-the-move.md | 1759 +++++++++++++++++ .../2026-09-20-shoot-on-the-move-design.md | 8 + 2 files changed, 1767 insertions(+) create mode 100644 docs/superpowers/plans/2026-09-20-shoot-on-the-move.md diff --git a/docs/superpowers/plans/2026-09-20-shoot-on-the-move.md b/docs/superpowers/plans/2026-09-20-shoot-on-the-move.md new file mode 100644 index 0000000..0185e9d --- /dev/null +++ b/docs/superpowers/plans/2026-09-20-shoot-on-the-move.md @@ -0,0 +1,1759 @@ +# Shoot on the Move Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Score Pollen into the up-facing Cell while the robot drives, using a precomputed (distance, radial velocity) → RPM table generated and visualised on a laptop, and a heading lead for tangential velocity computed on the robot. + +**Architecture:** One JavaScript physics module (`tools/shots/shots.js`) is shared by a browser visualiser (`index.html`) and a node exporter (`export.js`) that writes the generated `ShotTable.java`. On the robot a pure `ShotSolver` interpolates that table and leads the heading, a pure `TargetTracker` holds the Cell in field coordinates from timestamped tag detections and Pinpoint poses, and `Robot` wires them into the existing aim-assist and fire path. All pure classes have JVM tests; the ballistics has a self-check that gates export. + +**Tech Stack:** FTC SDK v12.0, FTCLib 2.1.1 core, Java 8, JUnit 4, goBILDA Pinpoint driver, VisionPortal + AprilTagProcessor; Node 18+ and a browser with Plotly (CDN) for the tools. + +**Spec:** `docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md` + +## Global Constraints + +- **Precondition:** the teleop plan (`docs/superpowers/plans/2026-09-20-biobuzz-teleop.md`) is fully executed. This plan modifies `Constants`, `ShooterSubsystem`, `OdometrySubsystem`, `VisionSubsystem` and `Robot` as that plan leaves them. If any of those files is missing, stop and execute the teleop plan first. +- SDK v12.0, no new Gradle dependencies. Java 8 source level, so no `var`, records, `List.of` or text blocks in TeamCode. +- Pure classes (`ShotSolver`, `TargetTracker`) import nothing from the SDK or Android. Only `ShotLog` and subsystems touch platform APIs. +- Units everywhere on the robot: metres, seconds, radians, RPM. The only degrees are gamepad-facing telemetry and the existing `aimRotation(bearingDeg)` helper. +- Frame: Pinpoint field frame as zeroed at init (X forward, Y left, heading CCW positive). Target and robot pose share it. No field constants for the Cell. +- Radial velocity is positive when closing on the Cell. Tangential velocity is positive when the robot moves to the left of the bearing. +- No Wi-Fi dashboards (manual R704). Tuning data goes to CSV on the hub. +- Commit prefixes: `feature:`, `docs:`, `chore:`. Every hardware-touching task is verified on the robot per the spec's ladder and recorded in the PR. +- Build and test commands need JDK 17 and the Android SDK (README "Requirements"). If they fail locally for environment reasons, push a branch and read CI. + +--- + +## File map + +Create: +- `tools/shots/shots.js`: ballistics, band search, grid sweep, Java export, self-check. The only copy of the physics. +- `tools/shots/index.html`: visualiser with the three 4414-style panels, form for inputs, export buttons. +- `tools/shots/export.js`: node script, `inputs.json` → `ShotTable.java`. +- `tools/shots/inputs.json`: the measured constants that generated the committed table. +- `tools/shots/README.md`: how to run the tools, the measurement procedure, and a table to record measurements. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java`: generated, committed. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java`: pure lookup and heading lead. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java`: pure pose ring buffer and target hold. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotLog.java`: CSV writer. +- `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java` +- `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java` + +Modify: +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java`: feed delay, camera transform, trim step, target expiry, log directory. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java`: variable target RPM and trim. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java`: field velocity. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java`: metre output, target position and frame time. +- `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java`: solver-driven aim, fire gate, trim, telemetry, logging. +- `README.md`: controls table, tools pointer, on-robot ladder. +- `docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md`: note the `shots.js` / `export.js` split. + +--- + +### Task 1: Ballistics module with self-check + +**Files:** +- Create: `tools/shots/shots.js` +- Create: `tools/shots/export.js` +- Create: `tools/shots/inputs.json` +- Modify: `docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md` + +**Interfaces:** +- Produces: `Shots.DEFAULTS`, `Shots.BALLS`, `Shots.axis(min, max, step)`, `Shots.simulate(rpm, radialVelMps, distM, p)` → `{points, result, tof}` with `result` one of `'score' | 'lip' | 'far' | 'short' | 'long'`, `Shots.band(distM, radialVelMps, p)` → `{lo, hi, rpm, band, tof}`, `Shots.solveGrid(p)` → `{dist[], vel[], rpm[][], band[][], tof[][], lo[][], hi[][]}` indexed `[distIdx][velIdx]`, `Shots.toJava(grid, p)` → string, `Shots.selfCheck()` → array of failure strings (empty is pass). `export.js` writes `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java` whose public statics are listed in Task 2. + +- [ ] **Step 1: Write `tools/shots/shots.js`** + +```js +// Ballistics, shot-table sweep and Java export for the BIOBUZZ shooter. +// Loaded by index.html in a browser and by export.js under node. This is the only copy of the physics. +(function (root, factory) { + if (typeof module === 'object' && module.exports) module.exports = factory(); + else root.Shots = factory(); +})(typeof self !== 'undefined' ? self : this, function () { + 'use strict'; + const G = 9.81; // m/s^2 + const RHO = 1.2; // kg/m^3, air + + const BALLS = { + POLLEN: { ballDiameterM: 0.0711, ballMassKg: 0.0249 }, // AndyMark am-5851: 2.80 in, 24.9 g + NECTAR: { ballDiameterM: 0.0919, ballMassKg: 0.0413 }, // AndyMark am-5852: 3.62 in, 41.3 g + }; + + const DEFAULTS = { + ball: 'POLLEN', + ballDiameterM: BALLS.POLLEN.ballDiameterM, + ballMassKg: BALLS.POLLEN.ballMassKg, + dragCd: 0.45, // guess; calibrate against measured landing distance + launchAngleDeg: 60, // measure on the robot + exitHeightM: 0.40, // measure on the robot + exitSpeedPerRpm: 0.0023, // m/s of ball per flywheel RPM; measure with slow-motion video + shooterOffsetM: 0.15, // ball exit point forward of the robot centre + rpmToleranceRpm: 50, // must match Constants.SHOOTER_TOLERANCE_RPM; the fixed hood leaves a 125-175 RPM band + lipHeightM: 1.359, // near lip of the up-facing Cell, 53.5 in + openingWidthM: 0.508, // 20 in + openingLengthM: 0.356, // 14 in along the tilted face + openingTiltDeg: 30, // face rises away from the robot; confirm against manual Figure 9-10 + marginM: 0.02, // clearance beyond the ball radius at each edge + distMinM: 0.6, distMaxM: 3.0, distStepM: 0.1, + velMinMps: -1.0, velMaxMps: 1.0, velStepMps: 0.1, + rpmMin: 1500, rpmMax: 5500, rpmStep: 25, + dtS: 0.002, + }; + + function axis(min, max, step) { + const out = []; + for (let i = 0; min + i * step <= max + 1e-9; i++) out.push(Math.round((min + i * step) * 1e6) / 1e6); + return out; + } + + // The opening as a segment in the shot plane: near lip at x = distM, far edge higher and further. + function opening(distM, p) { + const tilt = p.openingTiltDeg * Math.PI / 180; + return { + x0: distM, y0: p.lipHeightM, + x1: distM + p.openingLengthM * Math.cos(tilt), y1: p.lipHeightM + p.openingLengthM * Math.sin(tilt), + }; + } + + // Fraction 0..1 along the opening where segment a->b crosses it, or null if it does not. + function crossing(ax, ay, bx, by, o) { + const rx = bx - ax, ry = by - ay, sx = o.x1 - o.x0, sy = o.y1 - o.y0; + const den = rx * sy - ry * sx; + if (Math.abs(den) < 1e-12) return null; + const t = ((o.x0 - ax) * sy - (o.y0 - ay) * sx) / den; + const u = ((o.x0 - ax) * ry - (o.y0 - ay) * rx) / den; + return (t >= 0 && t <= 1 && u >= 0 && u <= 1) ? u : null; + } + + // Fly one ball from the shooter (x = 0, y = exit height) toward a Cell whose near lip is at x = distM. + // The robot's radial velocity adds to the ball's horizontal velocity. Quadratic drag, trapezoidal + // velocity step so the no-drag case reproduces the exact parabola. + function simulate(rpm, radialVelMps, distM, p) { + const r = p.ballDiameterM / 2; + const k = 0.5 * RHO * p.dragCd * Math.PI * r * r / p.ballMassKg; + const a = p.launchAngleDeg * Math.PI / 180, v0 = rpm * p.exitSpeedPerRpm; + const o = opening(distM, p); + const clear = (r + p.marginM) / p.openingLengthM; // edge clearance as a fraction of the opening + let x = 0, y = p.exitHeightM, vx = v0 * Math.cos(a) + radialVelMps, vy = v0 * Math.sin(a), t = 0; + const pts = [[x, y]]; + let result = 'short'; + while (t < 5) { + const v = Math.hypot(vx, vy); + const vx1 = vx - k * v * vx * p.dtS, vy1 = vy - (G + k * v * vy) * p.dtS; + const nx = x + (vx + vx1) / 2 * p.dtS, ny = y + (vy + vy1) / 2 * p.dtS; + vx = vx1; vy = vy1; t += p.dtS; + const u = crossing(x, y, nx, ny, o); + x = nx; y = ny; pts.push([x, y]); + if (u !== null && vy < 0) { result = u < clear ? 'lip' : u > 1 - clear ? 'far' : 'score'; break; } + if (x >= o.x0 && y < o.y0) { result = 'short'; break; } // into the front face below the lip + if (x > o.x1 && y >= o.y1) { result = 'long'; break; } // over the far edge + if (y < 0) { result = x > o.x1 ? 'long' : 'short'; break; } + } + return { points: pts, result: result, tof: t }; + } + + // The contiguous band of RPMs that score, its centre and the time of flight at the centre. + function band(distM, radialVelMps, p) { + let lo = NaN, hi = NaN; + for (const rpm of axis(p.rpmMin, p.rpmMax, p.rpmStep)) { + if (simulate(rpm, radialVelMps, distM, p).result === 'score') { if (Number.isNaN(lo)) lo = rpm; hi = rpm; } + } + if (Number.isNaN(lo)) return { lo: NaN, hi: NaN, rpm: NaN, band: NaN, tof: NaN }; + const centre = (lo + hi) / 2; + return { lo: lo, hi: hi, rpm: centre, band: hi - lo, tof: simulate(centre, radialVelMps, distM, p).tof }; + } + + // Sweep distance x radial velocity. A cell is usable when its band is at least twice the flywheel tolerance. + function solveGrid(p) { + const dist = axis(p.distMinM, p.distMaxM, p.distStepM), vel = axis(p.velMinMps, p.velMaxMps, p.velStepMps); + const g = { dist: dist, vel: vel, rpm: [], band: [], tof: [], lo: [], hi: [] }; + for (let i = 0; i < dist.length; i++) { + for (const key of ['rpm', 'band', 'tof', 'lo', 'hi']) g[key].push([]); + for (let j = 0; j < vel.length; j++) { + const b = band(dist[i], vel[j], p); + const usable = b.band >= 2 * p.rpmToleranceRpm; + g.rpm[i].push(usable ? b.rpm : NaN); + g.band[i].push(b.band); + g.tof[i].push(usable ? b.tof : NaN); + g.lo[i].push(b.lo); + g.hi[i].push(b.hi); + } + } + return g; + } + + function num(v) { return Number.isNaN(v) ? 'Double.NaN' : String(Math.round(v * 1000) / 1000); } + function row(a) { return '{' + a.map(num).join(', ') + '}'; } + function matrix(m) { return '{\n ' + m.map(row).join(',\n ') + '\n }'; } + + function toJava(g, p) { + return 'package org.firstinspires.ftc.teamcode;\n\n' + + '/**\n' + + ' * GENERATED by tools/shots (node tools/shots/export.js). Do not edit by hand: change tools/shots/inputs.json\n' + + ' * and regenerate. Rows are distance to the near lip, columns are radial robot velocity (positive closing).\n' + + ' * Inputs: ' + JSON.stringify(p) + '\n' + + ' */\n' + + 'public final class ShotTable {\n' + + ' private ShotTable() {}\n\n' + + ' public static final String BALL = "' + p.ball + '";\n' + + ' public static final double LAUNCH_ANGLE_DEG = ' + num(p.launchAngleDeg) + ';\n' + + ' public static final double EXIT_HEIGHT_M = ' + num(p.exitHeightM) + ';\n' + + ' public static final double EXIT_SPEED_PER_RPM = ' + p.exitSpeedPerRpm + ';\n' + + ' public static final double SHOOTER_OFFSET_M = ' + num(p.shooterOffsetM) + ';\n' + + ' public static final double OPENING_WIDTH_M = ' + num(p.openingWidthM) + ';\n' + + ' /** Ball radius plus edge margin: how far inside the opening\'s side edges the ball centre must pass. */\n' + + ' public static final double LATERAL_CLEARANCE_M = ' + num(p.ballDiameterM / 2 + p.marginM) + ';\n' + + ' public static final double MIN_BAND_RPM = ' + num(2 * p.rpmToleranceRpm) + ';\n\n' + + ' public static final double[] DIST_M = ' + row(g.dist) + ';\n' + + ' public static final double[] VEL_MPS = ' + row(g.vel) + ';\n' + + ' /** Band-centre flywheel RPM, [dist][vel]. NaN where no shot lands. */\n' + + ' public static final double[][] RPM = ' + matrix(g.rpm) + ';\n' + + ' /** Width of the valid RPM band, [dist][vel]. NaN where no shot lands. */\n' + + ' public static final double[][] BAND_RPM = ' + matrix(g.band) + ';\n' + + ' /** Time of flight at the band centre, seconds, [dist][vel]. */\n' + + ' public static final double[][] TOF_S = ' + matrix(g.tof) + ';\n' + + '}\n'; + } + + // Two checks that fail loudly if the physics is broken. Export is refused when this returns anything. + function selfCheck() { + const fails = []; + // 1. With no drag the flight must match the closed-form parabola to 1 mm. + const p = Object.assign({}, DEFAULTS, { dragCd: 0, lipHeightM: 100 }); // nothing to hit + const s = simulate(3000, 0.3, 2, p); + const a = p.launchAngleDeg * Math.PI / 180, v0 = 3000 * p.exitSpeedPerRpm; + for (const frac of [0.25, 0.5, 0.75]) { + const n = Math.floor((s.points.length - 1) * frac), t = n * p.dtS; + const ex = (v0 * Math.cos(a) + 0.3) * t, ey = p.exitHeightM + v0 * Math.sin(a) * t - 0.5 * G * t * t; + const err = Math.hypot(s.points[n][0] - ex, s.points[n][1] - ey); + if (err > 0.001) fails.push('no-drag flight is ' + (err * 1000).toFixed(2) + ' mm off the parabola at t=' + t.toFixed(3) + ' s'); + } + // 2. Standing still, a longer shot needs more RPM. + const g = solveGrid(Object.assign({}, DEFAULTS, { velMinMps: 0, velMaxMps: 0 })); + let last = -Infinity, any = false; + for (let i = 0; i < g.dist.length; i++) { + const r = g.rpm[i][0]; + if (Number.isNaN(r)) continue; + any = true; + if (r < last) fails.push('RPM falls with distance at ' + g.dist[i] + ' m'); + last = r; + } + if (!any) fails.push('no distance has a valid stationary shot with the default inputs'); + return fails; + } + + return { DEFAULTS: DEFAULTS, BALLS: BALLS, axis: axis, opening: opening, simulate: simulate, band: band, solveGrid: solveGrid, toJava: toJava, selfCheck: selfCheck }; +}); +``` + +- [ ] **Step 2: Write `tools/shots/export.js`** + +```js +#!/usr/bin/env node +// Regenerates TeamCode/.../ShotTable.java from tools/shots/inputs.json. Run from any directory: +// node tools/shots/export.js +const fs = require('fs'); +const path = require('path'); +const Shots = require('./shots.js'); + +const inputs = JSON.parse(fs.readFileSync(path.join(__dirname, 'inputs.json'), 'utf8')); +const p = Object.assign({}, Shots.DEFAULTS, inputs); + +const fails = Shots.selfCheck(); +if (fails.length) { + console.error('Self-check failed, not exporting:\n ' + fails.join('\n ')); + process.exit(1); +} + +const out = path.join(__dirname, '..', '..', 'TeamCode', 'src', 'main', 'java', 'org', 'firstinspires', 'ftc', 'teamcode', 'ShotTable.java'); +const grid = Shots.solveGrid(p); +fs.writeFileSync(out, Shots.toJava(grid, p)); +const usable = grid.rpm.flat().filter(v => !Number.isNaN(v)).length; +console.log('wrote ' + path.relative(process.cwd(), out) + ': ' + grid.dist.length + ' x ' + grid.vel.length + ' cells, ' + usable + ' usable'); +``` + +- [ ] **Step 3: Write `tools/shots/inputs.json`** + +Every physical input, copied from the defaults. This file is the record of what generated the committed table, so all keys are listed even while they equal the defaults. + +```json +{ + "ball": "POLLEN", + "ballDiameterM": 0.0711, + "ballMassKg": 0.0249, + "dragCd": 0.45, + "launchAngleDeg": 60, + "exitHeightM": 0.40, + "exitSpeedPerRpm": 0.0023, + "shooterOffsetM": 0.15, + "rpmToleranceRpm": 50, + "lipHeightM": 1.359, + "openingWidthM": 0.508, + "openingLengthM": 0.356, + "openingTiltDeg": 30, + "marginM": 0.02 +} +``` + +- [ ] **Step 4: Run the self-check and a spot simulation** + +```bash +node -e "const S=require('./tools/shots/shots.js'); console.log(S.selfCheck()); const b=S.band(2.0, 0, S.DEFAULTS); console.log(b); console.log(S.simulate(b.rpm, 0, 2.0, S.DEFAULTS).result);" +``` + +Expected: `[]`, then an object like `{ lo: 2600, hi: 2725, rpm: 2662.5, band: 125, tof: 0.756 }`, then `score`. + +The band is narrow on purpose. With a fixed launch angle the lower edge is set by the near lip and the upper edge by the far edge of the 14 in opening, and that comes out at 125 to 175 RPM for every distance and every launch angle between 45° and 75° with these inputs. That is why `rpmToleranceRpm` defaults to 50 and Task 6 tightens `Constants.SHOOTER_TOLERANCE_RPM` to match: the flywheel has to hold ±50 RPM or no cell is usable. If `selfCheck` reports "no distance has a valid stationary shot", something in `DEFAULTS` has been changed so that the band is under twice the tolerance; put it back rather than loosening the check. + +- [ ] **Step 5: Note the file split in the spec** + +In `docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md`, in the "Offline solver and visualiser" section, replace the sentence beginning "`tools/shots/index.html`, vanilla JavaScript with Plotly" up to "no build step." with: + +``` +`tools/shots/index.html`, vanilla JavaScript with Plotly from its CDN for the +plots (so it needs internet the first time; the browser caches it after). +Opened straight from the file system, no server, no build step. The physics +lives in `tools/shots/shots.js`, loaded by the page and by a node script +`tools/shots/export.js` that regenerates the Java table from +`tools/shots/inputs.json`, so the table can be rebuilt without a browser. A +``` + +- [ ] **Step 6: Commit** + +```bash +git add tools/shots docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md +git commit -m "feature: shot ballistics module, node exporter and inputs file" +``` + +--- + +### Task 2: Generate and commit the first `ShotTable.java` + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java` (generated) + +**Interfaces:** +- Consumes: `tools/shots/export.js` (Task 1). +- Produces: `ShotTable.BALL`, `LAUNCH_ANGLE_DEG`, `EXIT_HEIGHT_M`, `EXIT_SPEED_PER_RPM`, `SHOOTER_OFFSET_M`, `OPENING_WIDTH_M`, `LATERAL_CLEARANCE_M`, `MIN_BAND_RPM` (all `double` except `BALL`), `double[] DIST_M`, `double[] VEL_MPS`, `double[][] RPM`, `BAND_RPM`, `TOF_S` indexed `[dist][vel]`. Used by Task 6. + +- [ ] **Step 1: Generate** + +```bash +node tools/shots/export.js +head -30 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java +``` + +Expected: `wrote TeamCode/.../ShotTable.java: 25 x 21 cells, N usable` with N above 100, and a file that starts with the package line and the GENERATED comment. + +- [ ] **Step 2: Compile** + +```bash +./gradlew :TeamCode:compileDebugJavaWithJavac --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. A failure here means a `Double.NaN` or number formatting problem in `toJava`; fix `num()` in `shots.js`, regenerate, do not hand-edit the Java. + +- [ ] **Step 3: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java +git commit -m "feature: generated shot table from default inputs" +``` + +--- + +### Task 3: Visualiser page + +**Files:** +- Create: `tools/shots/index.html` + +**Interfaces:** +- Consumes: everything in `Shots` (Task 1). +- Produces: nothing for code. A person uses it to inspect the table and to export `ShotTable.java` and `inputs.json`. + +- [ ] **Step 1: Write `tools/shots/index.html`** + +```html + + + + +BIOBUZZ shot table + + + + + +

+

Ball

+ + +

Actions

+ + + + +

+
+
+ +
+ + + + +
+
+
+
+
+ + + +``` + +- [ ] **Step 2: Check it in a browser** + +Open `tools/shots/index.html` (double-click, or `xdg-open tools/shots/index.html`). Expected: no red banner; the form shows the defaults; after "Compute table" the status line shows the cell count and time, the fan shows a spread of red and green arcs with a black opening segment, the band chart shows the green fill closing toward the right, the heatmap has blank cells at high closing speed. Moving the sliders redraws. "Export ShotTable.java" downloads a file identical to the committed one when the form still holds the defaults: + +```bash +diff ~/Downloads/ShotTable.java TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java && echo SAME +``` + +Expected: `SAME`. + +If no browser is available to the executor, record that the visual check is pending and verify the page parses: + +```bash +node -e "const fs=require('fs'); const h=fs.readFileSync('tools/shots/index.html','utf8'); const js=h.split('')[0]; new Function(js.replace(/^'use strict';/, '')); console.log('script parses')" +``` + +Expected: `script parses`. + +- [ ] **Step 3: Commit** + +```bash +git add tools/shots/index.html +git commit -m "feature: shot table visualiser with trajectory fan, band chart and heatmap" +``` + +--- + +### Task 4: `ShotSolver`, tested + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java` +- Test: `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java` + +**Interfaces:** +- Produces: + - `ShotSolver(double[] dist, double[] vel, double[][] rpm, double[][] band, double[][] tof, double launchAngleDeg, double exitSpeedPerRpm, double shooterOffsetM, double feedDelayS, double minBandRpm, double openingWidthM, double lateralClearanceM, double fallbackRpm)` + - `ShotSolver.Shot solve(double x, double y, double headingRad, double vx, double vy, double targetX, double targetY)` + - `ShotSolver.Shot` fields: `boolean valid`, `String reason` (`"OK"`, `"OUT OF RANGE"`, `"CLOSING TOO FAST"`, `"BACKING TOO FAST"`, `"NO SHOT"`), `double rpm`, `idleRpm`, `headingRad`, `headingToleranceRad`, `distanceM`, `radialVel`, `tangentialVel`, `timeOfFlightS`, `bandRpm`; method `double headingErrorRad(double currentHeadingRad)` (positive means turn left). + - `static double interpolate(double[] xs, double[] ys, double[][] v, double x, double y)` returning `Double.NaN` outside the grid or when any of the four corners is NaN. + - `static double wrap(double rad)` to (-pi, pi]. + +- [ ] **Step 1: Write the failing tests** + +```java +package org.firstinspires.ftc.teamcode; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertTrue; + +import org.junit.Test; + +public class ShotSolverTest { + private static final double EPS = 1e-9; + + // rpm = 2000 + 500 * (d - 1) - 200 * vr on a 3 x 3 grid; the (3 m, +1 m/s) cell has no shot. + private static final double[] DIST = {1, 2, 3}; + private static final double[] VEL = {-1, 0, 1}; + private static final double[][] RPM = { + {2200, 2000, 1800}, + {2700, 2500, 2300}, + {3200, 3000, Double.NaN}}; + private static final double[][] BAND = { + {400, 400, 400}, + {400, 400, 100}, + {400, 400, Double.NaN}}; + private static final double[][] TOF = { + {0.8, 0.8, 0.8}, + {1.0, 1.0, 1.0}, + {1.2, 1.2, Double.NaN}}; + + private static ShotSolver solver(double shooterOffsetM, double feedDelayS) { + // 60 deg launch, 0.002 m/s per RPM: 2500 RPM gives 5 m/s exit, 2.5 m/s horizontal. + return new ShotSolver(DIST, VEL, RPM, BAND, TOF, 60, 0.002, shooterOffsetM, feedDelayS, 200, 0.508, 0.0555, 3500); + } + + @Test + public void interpolateReturnsGridValueAtGridPoint() { + assertEquals(2500, ShotSolver.interpolate(DIST, VEL, RPM, 2, 0), EPS); + } + + @Test + public void interpolateAveragesNeighboursAtMidpoint() { + // between (1,-1)=2200, (1,0)=2000, (2,-1)=2700, (2,0)=2500 + assertEquals(2350, ShotSolver.interpolate(DIST, VEL, RPM, 1.5, -0.5), EPS); + } + + @Test + public void interpolateOutsideGridIsNaN() { + assertTrue(Double.isNaN(ShotSolver.interpolate(DIST, VEL, RPM, 0.5, 0))); + assertTrue(Double.isNaN(ShotSolver.interpolate(DIST, VEL, RPM, 2, 1.5))); + } + + @Test + public void interpolateNextToMissingCellIsNaN() { + assertTrue(Double.isNaN(ShotSolver.interpolate(DIST, VEL, RPM, 2.5, 0.5))); + } + + @Test + public void stationaryShotAimsAlongBearingAtTableRpm() { + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 0, 0, 2, 0); + assertTrue(s.valid); + assertEquals("OK", s.reason); + assertEquals(2, s.distanceM, EPS); + assertEquals(0, s.headingRad, EPS); + assertEquals(2500, s.rpm, EPS); + assertEquals(1.0, s.timeOfFlightS, EPS); + } + + @Test + public void bearingFollowsTargetPosition() { + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 0, 0, 0, 2); + assertEquals(Math.PI / 2, s.headingRad, EPS); + assertEquals(0, s.radialVel, EPS); + } + + @Test + public void strafingLeftLeadsRightOfBearing() { + // Target straight ahead, robot moving left (+y) at 0.5 m/s. Horizontal exit speed 2.5 m/s. + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 0, 0.5, 2, 0); + assertEquals(0.5, s.tangentialVel, EPS); + assertEquals(0, s.radialVel, EPS); + assertEquals(-Math.asin(0.5 / 2.5), s.headingRad, 1e-9); + } + + @Test + public void closingLowersRpmAndCountsAsRadial() { + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 0.5, 0, 2, 0); + assertEquals(0.5, s.radialVel, EPS); + assertEquals(2400, s.rpm, EPS); // 2500 - 200 * 0.5 + assertTrue(s.valid); + } + + @Test + public void closingTooFastIsInvalidWithReason() { + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 2.0, 0, 2, 0); + assertFalse(s.valid); + assertEquals("CLOSING TOO FAST", s.reason); + assertEquals(2500, s.idleRpm, EPS); // stationary RPM at the same distance + } + + @Test + public void backingTooFastIsInvalidWithReason() { + assertEquals("BACKING TOO FAST", solver(0, 0).solve(0, 0, 0, -2.0, 0, 2, 0).reason); + } + + @Test + public void outOfRangeIsInvalidAndIdlesAtNearestEdge() { + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 0, 0, 5, 0); + assertFalse(s.valid); + assertEquals("OUT OF RANGE", s.reason); + assertEquals(3000, s.idleRpm, EPS); // stationary RPM at the far edge of the grid + } + + @Test + public void idleFallsBackWhenTheStationaryCellIsMissing() { + double[][] noStationary = {{2200, Double.NaN, 1800}, {2700, Double.NaN, 2300}, {3200, Double.NaN, Double.NaN}}; + ShotSolver s = new ShotSolver(DIST, VEL, noStationary, BAND, TOF, 60, 0.002, 0, 0, 200, 0.508, 0.0555, 3500); + assertEquals(3500, s.solve(0, 0, 0, 0, 0, 2, 0).idleRpm, EPS); + } + + @Test + public void narrowBandIsInvalid() { + // (2 m, +1 m/s) has a 100 RPM band, below the 200 RPM minimum. + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 1.0, 0, 2, 0); + assertFalse(s.valid); + assertEquals("NO SHOT", s.reason); + } + + @Test + public void feedDelayAdvancesThePose() { + ShotSolver.Shot s = solver(0, 0.5).solve(0, 0, 0, 1.0, 0, 2, 0); + assertEquals(1.5, s.distanceM, EPS); + } + + @Test + public void shooterOffsetShortensDistanceAlongHeading() { + ShotSolver.Shot s = solver(0.2, 0).solve(0, 0, 0, 0, 0, 2, 0); + assertEquals(1.8, s.distanceM, EPS); + } + + @Test + public void headingToleranceNarrowsWithDistance() { + ShotSolver.Shot near = solver(0, 0).solve(0, 0, 0, 0, 0, 1, 0); + ShotSolver.Shot far = solver(0, 0).solve(0, 0, 0, 0, 0, 3, 0); + assertEquals(Math.atan((0.254 - 0.0555) / 1.0), near.headingToleranceRad, EPS); + assertTrue(far.headingToleranceRad < near.headingToleranceRad); + } + + @Test + public void headingErrorWrapsAndIsPositiveWhenTargetIsLeft() { + ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 0, 0, 0, 2); // target heading +90 deg + assertEquals(Math.PI / 2, s.headingErrorRad(0), EPS); + assertEquals(-Math.PI / 2, s.headingErrorRad(Math.PI), EPS); + assertEquals(0.1, ShotSolver.wrap(2 * Math.PI + 0.1), EPS); + } +} +``` + +- [ ] **Step 2: Run the tests to see them fail** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*ShotSolverTest' --no-daemon +``` + +Expected: compile failure, `cannot find symbol ShotSolver`. + +- [ ] **Step 3: Write `ShotSolver.java`** + +```java +package org.firstinspires.ftc.teamcode; + +/** + * Turns robot pose, robot velocity and the Cell position into a flywheel RPM and a heading. + * + * The table (generated by tools/shots) already contains the effect of the robot's radial velocity on + * the ball, so the only lookahead here is the indexer feed delay. Tangential velocity becomes a heading + * lead: the robot turns away from its sideways motion so the ball's horizontal velocity (exit plus robot) + * points at the Cell. Pure maths, no SDK types, unit tested. + */ +public final class ShotSolver { + public static final class Shot { + public final boolean valid; + public final String reason; + /** Flywheel target when valid. */ + public final double rpm; + /** Flywheel target to hold when not valid: the stationary RPM at this distance clamped to the grid, or the fallback if that cell is empty. */ + public final double idleRpm; + /** Field heading the robot must point to, radians. */ + public final double headingRad; + /** Heading error allowed by the Cell width at this distance, radians. */ + public final double headingToleranceRad; + public final double distanceM; + public final double radialVel; + public final double tangentialVel; + public final double timeOfFlightS; + public final double bandRpm; + + Shot(boolean valid, String reason, double rpm, double idleRpm, double headingRad, double headingToleranceRad, + double distanceM, double radialVel, double tangentialVel, double timeOfFlightS, double bandRpm) { + this.valid = valid; + this.reason = reason; + this.rpm = rpm; + this.idleRpm = idleRpm; + this.headingRad = headingRad; + this.headingToleranceRad = headingToleranceRad; + this.distanceM = distanceM; + this.radialVel = radialVel; + this.tangentialVel = tangentialVel; + this.timeOfFlightS = timeOfFlightS; + this.bandRpm = bandRpm; + } + + /** How far to turn, radians, positive to the left (anticlockwise). */ + public double headingErrorRad(double currentHeadingRad) { + return wrap(headingRad - currentHeadingRad); + } + } + + private final double[] dist; + private final double[] vel; + private final double[][] rpm; + private final double[][] band; + private final double[][] tof; + private final double launchAngleRad; + private final double exitSpeedPerRpm; + private final double shooterOffsetM; + private final double feedDelayS; + private final double minBandRpm; + private final double halfOpeningM; + private final double fallbackRpm; + + public ShotSolver(double[] dist, double[] vel, double[][] rpm, double[][] band, double[][] tof, + double launchAngleDeg, double exitSpeedPerRpm, double shooterOffsetM, double feedDelayS, + double minBandRpm, double openingWidthM, double lateralClearanceM, double fallbackRpm) { + this.dist = dist; + this.vel = vel; + this.rpm = rpm; + this.band = band; + this.tof = tof; + this.launchAngleRad = Math.toRadians(launchAngleDeg); + this.exitSpeedPerRpm = exitSpeedPerRpm; + this.shooterOffsetM = shooterOffsetM; + this.feedDelayS = feedDelayS; + this.minBandRpm = minBandRpm; + this.halfOpeningM = openingWidthM / 2 - lateralClearanceM; + this.fallbackRpm = fallbackRpm; + } + + /** + * @param x, y, headingRad robot pose, field frame, metres and radians + * @param vx, vy robot velocity, field frame, m/s + * @param targetX, targetY Cell opening centre, field frame + */ + public Shot solve(double x, double y, double headingRad, double vx, double vy, double targetX, double targetY) { + // 1. Where the robot will be when the ball leaves. + double px = x + vx * feedDelayS; + double py = y + vy * feedDelayS; + // 2. Where the ball leaves from. + double sx = px + shooterOffsetM * Math.cos(headingRad); + double sy = py + shooterOffsetM * Math.sin(headingRad); + double dx = targetX - sx; + double dy = targetY - sy; + double d = Math.hypot(dx, dy); + double bearing = Math.atan2(dy, dx); + // 3. Robot velocity along and across the line to the Cell. + double ux = dx / d; + double uy = dy / d; + double radial = vx * ux + vy * uy; + double tangential = -vx * uy + vy * ux; + // 4. Table lookup. + double tableRpm = interpolate(dist, vel, rpm, d, radial); + double tableBand = interpolate(dist, vel, band, d, radial); + double tableTof = interpolate(dist, vel, tof, d, radial); + double stationary = interpolate(dist, vel, rpm, Math.max(dist[0], Math.min(dist[dist.length - 1], d)), 0); + double idle = Double.isNaN(stationary) ? fallbackRpm : stationary; + double tolerance = Math.atan(halfOpeningM / d); + + String reason = "OK"; + if (d < dist[0] || d > dist[dist.length - 1]) reason = "OUT OF RANGE"; + else if (radial > vel[vel.length - 1]) reason = "CLOSING TOO FAST"; + else if (radial < vel[0]) reason = "BACKING TOO FAST"; + else if (Double.isNaN(tableRpm) || Double.isNaN(tableBand) || tableBand < minBandRpm) reason = "NO SHOT"; + if (!reason.equals("OK")) { + return new Shot(false, reason, Double.NaN, idle, bearing, tolerance, d, radial, tangential, Double.NaN, tableBand); + } + // 5. Heading lead so exit velocity plus robot velocity points along the bearing. + double horizontalExit = tableRpm * exitSpeedPerRpm * Math.cos(launchAngleRad); + double lead = Math.asin(Math.max(-1, Math.min(1, tangential / horizontalExit))); + return new Shot(true, reason, tableRpm, idle, wrap(bearing - lead), tolerance, d, radial, tangential, tableTof, tableBand); + } + + /** Bilinear interpolation on a grid v[xIndex][yIndex]. NaN outside the grid or beside a NaN corner. */ + static double interpolate(double[] xs, double[] ys, double[][] v, double x, double y) { + if (x < xs[0] || x > xs[xs.length - 1] || y < ys[0] || y > ys[ys.length - 1]) return Double.NaN; + int i = upperIndex(xs, x); + int j = upperIndex(ys, y); + double tx = (x - xs[i - 1]) / (xs[i] - xs[i - 1]); + double ty = (y - ys[j - 1]) / (ys[j] - ys[j - 1]); + double a = v[i - 1][j - 1], b = v[i][j - 1], c = v[i - 1][j], e = v[i][j]; + if (Double.isNaN(a) || Double.isNaN(b) || Double.isNaN(c) || Double.isNaN(e)) return Double.NaN; + double low = a + (b - a) * tx; + double high = c + (e - c) * tx; + return low + (high - low) * ty; + } + + /** Index i in [1, n-1] with axis[i-1] <= value <= axis[i]. */ + private static int upperIndex(double[] axis, double value) { + int i = 1; + while (i < axis.length - 1 && axis[i] < value) i++; + return i; + } + + /** Wraps an angle to (-pi, pi]. */ + static double wrap(double rad) { + double r = rad % (2 * Math.PI); + if (r <= -Math.PI) r += 2 * Math.PI; + if (r > Math.PI) r -= 2 * Math.PI; + return r; + } +} +``` + +- [ ] **Step 4: Run the tests** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*ShotSolverTest' --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`, 17 tests passed. + +- [ ] **Step 5: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java +git commit -m "feature: shot solver with table lookup and heading lead, tested" +``` + +--- + +### Task 5: `TargetTracker`, tested + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java` +- Test: `TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java` + +**Interfaces:** +- Produces: + - `TargetTracker(int capacity, double camForwardM, double camLeftM, double camYawRad, double camPitchRad, double expiryS)` + - `void recordPose(long nanos, double x, double y, double headingRad)` + - `boolean update(long frameNanos, double camXRightM, double camYForwardM, double camZUpM)`: camera-frame position of the Cell centre from `ftcPose` (x right, y forward along the lens axis, z up), returns false when no pose has been recorded yet. + - `boolean hasTarget(long nowNanos)`, `double getX()`, `double getY()`, `double ageS(long nowNanos)`. + +- [ ] **Step 1: Write the failing tests** + +```java +package org.firstinspires.ftc.teamcode; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertTrue; + +import org.junit.Test; + +public class TargetTrackerTest { + private static final double EPS = 1e-9; + private static final long S = 1_000_000_000L; + + @Test + public void noPoseYetMeansNoUpdate() { + TargetTracker t = new TargetTracker(8, 0, 0, 0, 0, 5); + assertFalse(t.update(0, 0, 2, 0)); + assertFalse(t.hasTarget(0)); + } + + @Test + public void placesTargetFromCameraFrameAheadOfRobot() { + TargetTracker t = new TargetTracker(8, 0, 0, 0, 0, 5); + t.recordPose(0, 1, 1, 0); + assertTrue(t.update(0, 0, 2, 0)); + assertEquals(3, t.getX(), EPS); + assertEquals(1, t.getY(), EPS); + } + + @Test + public void usesPoseAtFrameTimeNotLatest() { + TargetTracker t = new TargetTracker(8, 0, 0, 0, 0, 5); + t.recordPose(0, 0, 0, 0); + t.recordPose(S, 1, 0, 0); // robot moved 1 m forward in the second after the frame + t.update(0, 0, 2, 0); // frame taken at t = 0 saw the Cell 2 m ahead + assertEquals(2, t.getX(), EPS); + } + + @Test + public void picksNearestPoseInTime() { + TargetTracker t = new TargetTracker(8, 0, 0, 0, 0, 5); + t.recordPose(0, 0, 0, 0); + t.recordPose(S, 1, 0, 0); + t.update(S - S / 10, 0, 2, 0); // 0.9 s: nearer the second pose + assertEquals(3, t.getX(), EPS); + } + + @Test + public void ringBufferKeepsOnlyTheLastCapacityPoses() { + TargetTracker t = new TargetTracker(2, 0, 0, 0, 0, 5); + t.recordPose(0, 0, 0, 0); + t.recordPose(S, 1, 0, 0); + t.recordPose(2 * S, 2, 0, 0); // evicts the t = 0 pose + t.update(0, 0, 2, 0); // nearest surviving pose is t = 1 s + assertEquals(3, t.getX(), EPS); + } + + @Test + public void appliesRobotHeading() { + TargetTracker t = new TargetTracker(8, 0, 0, 0, 0, 5); + t.recordPose(0, 0, 0, Math.PI / 2); // robot facing +y + t.update(0, 0, 2, 0); + assertEquals(0, t.getX(), EPS); + assertEquals(2, t.getY(), EPS); + } + + @Test + public void appliesCameraOffsetAndYaw() { + // Camera 0.1 m forward of centre, turned 90 deg left. Cell 1 m ahead of the camera is 1 m to the robot's left. + TargetTracker t = new TargetTracker(8, 0.1, 0, Math.PI / 2, 0, 5); + t.recordPose(0, 0, 0, 0); + t.update(0, 0, 1, 0); + assertEquals(0.1, t.getX(), EPS); + assertEquals(1, t.getY(), EPS); + } + + @Test + public void cameraXRightIsRobotNegativeY() { + TargetTracker t = new TargetTracker(8, 0, 0, 0, 0, 5); + t.recordPose(0, 0, 0, 0); + t.update(0, 0.5, 2, 0); + assertEquals(-0.5, t.getY(), EPS); + } + + @Test + public void pitchLevelsTheRange() { + // Camera pitched up 30 deg. A Cell 1 m along the lens axis is cos(30) ahead on the floor plan. + TargetTracker t = new TargetTracker(8, 0, 0, 0, Math.toRadians(30), 5); + t.recordPose(0, 0, 0, 0); + t.update(0, 0, 1, 0); + assertEquals(Math.cos(Math.toRadians(30)), t.getX(), EPS); + // A point straight up the camera's z axis is behind the lens axis on the floor plan. + t.update(0, 0, 0, 1); + assertEquals(-Math.sin(Math.toRadians(30)), t.getX(), EPS); + } + + @Test + public void expiresAfterConfiguredAge() { + TargetTracker t = new TargetTracker(8, 0, 0, 0, 0, 5); + t.recordPose(0, 0, 0, 0); + t.update(0, 0, 2, 0); + assertTrue(t.hasTarget(4 * S)); + assertEquals(4, t.ageS(4 * S), EPS); + assertFalse(t.hasTarget(6 * S)); + } +} +``` + +- [ ] **Step 2: Run the tests to see them fail** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*TargetTrackerTest' --no-daemon +``` + +Expected: compile failure, `cannot find symbol TargetTracker`. + +- [ ] **Step 3: Write `TargetTracker.java`** + +```java +package org.firstinspires.ftc.teamcode; + +/** + * Holds the up-facing Cell's opening centre in field coordinates. + * + * Camera frames arrive late, so each detection is placed using the robot pose recorded nearest the + * frame's timestamp, not the current pose. Between detections the stored point stands and the Pinpoint + * carries the aim. The Cells move when the Hive tips, so nothing here is a field constant. Pure maths. + */ +public final class TargetTracker { + private final long[] poseNanos; + private final double[] poseX; + private final double[] poseY; + private final double[] poseHeading; + private int next; + private int count; + + private final double camForwardM; + private final double camLeftM; + private final double camYawRad; + private final double camPitchRad; + private final long expiryNanos; + + private double targetX; + private double targetY; + private long seenNanos; + private boolean seen; + + /** + * @param capacity poses kept; 50 covers a second at a 50 Hz loop + * @param camForwardM camera lens forward of the robot centre + * @param camLeftM camera lens left of the robot centre + * @param camYawRad camera turned left of robot forward + * @param camPitchRad camera tilted up from level + * @param expiryS how long a detection stays usable + */ + public TargetTracker(int capacity, double camForwardM, double camLeftM, double camYawRad, double camPitchRad, double expiryS) { + poseNanos = new long[capacity]; + poseX = new double[capacity]; + poseY = new double[capacity]; + poseHeading = new double[capacity]; + this.camForwardM = camForwardM; + this.camLeftM = camLeftM; + this.camYawRad = camYawRad; + this.camPitchRad = camPitchRad; + this.expiryNanos = (long) (expiryS * 1e9); + } + + /** Call once per loop with the Pinpoint pose. */ + public void recordPose(long nanos, double x, double y, double headingRad) { + poseNanos[next] = nanos; + poseX[next] = x; + poseY[next] = y; + poseHeading[next] = headingRad; + next = (next + 1) % poseNanos.length; + if (count < poseNanos.length) count++; + } + + /** + * Places the Cell from a detection. Camera frame per the SDK's ftcPose: x right, y forward along the + * lens axis, z up. Returns false if no pose has been recorded yet. + */ + public boolean update(long frameNanos, double camXRightM, double camYForwardM, double camZUpM) { + if (count == 0) return false; + int p = nearestPose(frameNanos); + + // Camera frame to a level frame: undo the pitch, then x-right becomes y-left negative. + double forward = camYForwardM * Math.cos(camPitchRad) - camZUpM * Math.sin(camPitchRad); + double left = -camXRightM; + // Level camera frame to robot frame: yaw, then lens offset. + double rx = camForwardM + forward * Math.cos(camYawRad) - left * Math.sin(camYawRad); + double ry = camLeftM + forward * Math.sin(camYawRad) + left * Math.cos(camYawRad); + // Robot frame to field frame with the pose at the frame time. + double h = poseHeading[p]; + targetX = poseX[p] + rx * Math.cos(h) - ry * Math.sin(h); + targetY = poseY[p] + rx * Math.sin(h) + ry * Math.cos(h); + seenNanos = frameNanos; + seen = true; + return true; + } + + private int nearestPose(long nanos) { + int best = 0; + long bestGap = Long.MAX_VALUE; + for (int i = 0; i < count; i++) { + long gap = Math.abs(poseNanos[i] - nanos); + if (gap < bestGap) { + bestGap = gap; + best = i; + } + } + return best; + } + + public boolean hasTarget(long nowNanos) { + return seen && nowNanos - seenNanos <= expiryNanos; + } + + public double getX() { + return targetX; + } + + public double getY() { + return targetY; + } + + public double ageS(long nowNanos) { + return seen ? (nowNanos - seenNanos) / 1e9 : Double.POSITIVE_INFINITY; + } +} +``` + +- [ ] **Step 4: Run the tests** + +```bash +./gradlew :TeamCode:testDebugUnitTest --tests '*TargetTrackerTest' --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`, 10 tests passed. + +- [ ] **Step 5: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java +git commit -m "feature: target tracker placing the cell from timestamped detections, tested" +``` + +--- + +### Task 6: Constants, shooter target RPM, odometry velocity, vision position + +**Files:** +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java` +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java` + +**Interfaces:** +- Consumes: the teleop plan's versions of these files. +- Produces: `Constants.FEED_DELAY_S`, `CAMERA_FORWARD_M`, `CAMERA_LEFT_M`, `CAMERA_YAW_RAD`, `CAMERA_PITCH_RAD`, `TARGET_EXPIRY_S`, `POSE_BUFFER_SIZE`, `SHOOTER_TRIM_STEP_RPM`, `SHOT_LOG_DIR`; `ShooterSubsystem.setTargetRpm(double)`, `getTargetRpm()`, `trim(double)`, `getTrimRpm()`; `OdometrySubsystem.getVelocity()` → `double[]{vx, vy}` m/s field frame; `VisionSubsystem.getTargetX()`, `getTargetY()`, `getTargetZ()` (metres, camera frame), `getTargetFrameNanos()`. + +- [ ] **Step 1: Add to `Constants.java`** + +Change the shooter tolerance. The generated table only has cells where the RPM band is at least twice this, and the Cell geometry leaves a 125 to 175 RPM band, so 100 RPM would reject every cell: + +```java + public static final double SHOOTER_TOLERANCE_RPM = 50.0; // the shot table needs the wheel within this; see tools/shots/README.md +``` + +Then append inside the class, after the aim-assist block: + +```java + // Shoot on the move. Shooter geometry and the table live in the generated ShotTable. + public static final double FEED_DELAY_S = 0.15; // indexer feed command to ball exit, measure on the robot + public static final double SHOOTER_TRIM_STEP_RPM = 50.0; // D-pad up/down + public static final double TARGET_EXPIRY_S = 5.0; // how long the last tag fix is trusted + public static final int POSE_BUFFER_SIZE = 50; // about one second of loops + + // Camera lens relative to the robot centre. Fill in from the CAD once the mount is designed. + public static final double CAMERA_FORWARD_M = 0.0; + public static final double CAMERA_LEFT_M = 0.0; + public static final double CAMERA_YAW_RAD = 0.0; // positive turned left + public static final double CAMERA_PITCH_RAD = 0.0; // positive tilted up + + public static final String SHOT_LOG_DIR = "/sdcard/FIRST/shots"; +``` + +- [ ] **Step 2: Make the shooter target adjustable in `ShooterSubsystem.java`** + +Replace the field `private boolean running;` with: + +```java + private boolean running; + private double targetRpm = Constants.SHOOTER_SETPOINT_RPM; + private double trimRpm = 0; +``` + +Replace `spinUp()` and `atSpeed()` with: + +```java + public void spinUp() { + running = true; + double tps = ShooterMath.rpmToTicksPerSecond(getTargetRpm(), Constants.SHOOTER_TICKS_PER_REV); + left.setVelocity(tps); + right.setVelocity(tps); + } + + /** Changes the target. Re-sends the velocity command only if the target moved and the wheels are running. */ + public void setTargetRpm(double rpm) { + if (rpm == targetRpm) return; + targetRpm = rpm; + if (running) spinUp(); + } + + /** Target plus the driver's trim. */ + public double getTargetRpm() { + return targetRpm + trimRpm; + } + + /** Driver adjustment applied to every target for the rest of the run. */ + public void trim(double deltaRpm) { + trimRpm += deltaRpm; + if (running) spinUp(); + } + + public double getTrimRpm() { + return trimRpm; + } + + /** True when both wheels are within tolerance of the current target. */ + public boolean atSpeed() { + double target = getTargetRpm(); + return running + && Math.abs(getLeftRpm() - target) < Constants.SHOOTER_TOLERANCE_RPM + && Math.abs(getRightRpm() - target) < Constants.SHOOTER_TOLERANCE_RPM; + } +``` + +- [ ] **Step 3: Expose velocity from `OdometrySubsystem.java`** + +Add after `getPose()`: + +```java + /** Field-frame velocity from the Pinpoint, metres per second, as {x, y}. */ + public double[] getVelocity() { + return new double[]{pinpoint.getVelX(DistanceUnit.METER), pinpoint.getVelY(DistanceUnit.METER)}; + } +``` + +- [ ] **Step 4: Expose the target position and frame time from `VisionSubsystem.java`** + +In the constructor, change the processor builder to output metres: + +```java + processor = new AprilTagProcessor.Builder() + .setTagLibrary(AprilTagGameDatabase.getCurrentGameTagLibrary()) + .setOutputUnits(DistanceUnit.METER, AngleUnit.DEGREES) + .build(); +``` + +Add imports: + +```java +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +``` + +Add after `getBearingDeg()`: + +```java + /** Cell centre right of the lens, metres. Only valid when hasTarget(). */ + public double getTargetX() { + return target.ftcPose.x; + } + + /** Cell centre forward along the lens axis, metres. Only valid when hasTarget(). */ + public double getTargetY() { + return target.ftcPose.y; + } + + /** Cell centre above the lens axis, metres. Only valid when hasTarget(). */ + public double getTargetZ() { + return target.ftcPose.z; + } + + /** System.nanoTime() when the frame holding this detection was captured. */ + public long getTargetFrameNanos() { + return target.frameAcquisitionNanoTime; + } +``` + +- [ ] **Step 5: Build and run all tests** + +```bash +./gradlew :TeamCode:assembleDebug :TeamCode:testDebugUnitTest --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 6: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode +git commit -m "feature: adjustable shooter target with trim, odometry velocity, vision target position" +``` + +--- + +### Task 7: `ShotLog` CSV writer + +**Files:** +- Create: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotLog.java` + +**Interfaces:** +- Produces: `ShotLog.open(String dir)` → `ShotLog` or `null` if the file cannot be created; `void row(double... values)`; `void close()`; `String fileName()`. Column order is fixed by `HEADER`. + +- [ ] **Step 1: Write `ShotLog.java`** + +```java +package org.firstinspires.ftc.teamcode; + +import android.annotation.SuppressLint; + +import java.io.BufferedWriter; +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.Locale; + +/** + * One CSV per OpMode run under /sdcard/FIRST/shots, pulled off the hub with adb after practice. + * Wi-Fi dashboards are banned at events (R704), so this is the tuning record. + */ +public final class ShotLog { + public static final String HEADER = "t_s,x_m,y_m,heading_rad,vx_mps,vy_mps,dist_m,radial_mps,tangential_mps," + + "table_rpm,left_rpm,right_rpm,heading_err_rad,valid,fired"; + private static final int FLUSH_EVERY = 50; + + private final BufferedWriter out; + private final String fileName; + private final long startNanos = System.nanoTime(); + private int rows; + + private ShotLog(BufferedWriter out, String fileName) { + this.out = out; + this.fileName = fileName; + } + + /** Creates the directory and a timestamped file. Returns null, and never throws, if that fails. */ + @SuppressLint("SimpleDateFormat") + public static ShotLog open(String dir) { + try { + File folder = new File(dir); + if (!folder.isDirectory() && !folder.mkdirs()) return null; + String name = new SimpleDateFormat("yyyyMMdd-HHmmss", Locale.US).format(new Date()) + ".csv"; + BufferedWriter w = new BufferedWriter(new FileWriter(new File(folder, name))); + w.write(HEADER); + w.newLine(); + return new ShotLog(w, name); + } catch (IOException e) { + return null; + } + } + + /** Appends one row. The first column, elapsed seconds, is added here. */ + public void row(double... values) { + try { + StringBuilder sb = new StringBuilder(); + sb.append(String.format(Locale.US, "%.3f", (System.nanoTime() - startNanos) / 1e9)); + for (double v : values) sb.append(',').append(String.format(Locale.US, "%.4f", v)); + out.write(sb.toString()); + out.newLine(); + if (++rows % FLUSH_EVERY == 0) out.flush(); + } catch (IOException ignored) { + // A failed log line must never stop the robot. + } + } + + public void close() { + try { + out.flush(); + out.close(); + } catch (IOException ignored) { + } + } + + public String fileName() { + return fileName; + } +} +``` + +- [ ] **Step 2: Build** + +```bash +./gradlew :TeamCode:assembleDebug --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 3: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotLog.java +git commit -m "feature: CSV shot log on the hub" +``` + +--- + +### Task 8: Wire it into `Robot` + +**Files:** +- Modify: `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java` + +**Interfaces:** +- Consumes: `ShotTable` (Task 2), `ShotSolver` (Task 4), `TargetTracker` (Task 5), `Constants`, `ShooterSubsystem`, `OdometrySubsystem`, `VisionSubsystem` (Task 6), `ShotLog` (Task 7), and the teleop plan's `Robot` with `aimRotation(double)`, `intake`, `indexer`, `battery`. +- Produces: the driver-facing behaviour in the README controls table (Task 9). + +- [ ] **Step 1: Add fields and construction** + +Add imports: + +```java +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +``` + +Add fields after `public final VisionSubsystem vision;`: + +```java + private final ShotSolver solver = new ShotSolver( + ShotTable.DIST_M, ShotTable.VEL_MPS, ShotTable.RPM, ShotTable.BAND_RPM, ShotTable.TOF_S, + ShotTable.LAUNCH_ANGLE_DEG, ShotTable.EXIT_SPEED_PER_RPM, ShotTable.SHOOTER_OFFSET_M, + Constants.FEED_DELAY_S, ShotTable.MIN_BAND_RPM, ShotTable.OPENING_WIDTH_M, ShotTable.LATERAL_CLEARANCE_M, + Constants.SHOOTER_SETPOINT_RPM); + private final TargetTracker tracker = new TargetTracker( + Constants.POSE_BUFFER_SIZE, Constants.CAMERA_FORWARD_M, Constants.CAMERA_LEFT_M, + Constants.CAMERA_YAW_RAD, Constants.CAMERA_PITCH_RAD, Constants.TARGET_EXPIRY_S); + private ShotLog log; + private ShotSolver.Shot lastShot; +``` + +- [ ] **Step 2: Open and close the log** + +In `start()`, after `vision.stopLiveView();`: + +```java + log = ShotLog.open(Constants.SHOT_LOG_DIR); +``` + +In `stop()`, before `shooter.idle();`: + +```java + if (log != null) log.close(); +``` + +- [ ] **Step 3: Replace `periodic()`** + +Replace the whole method with: + +```java + /** Called repeatedly while the OpMode is running. */ + public void periodic() { + driver.readButtons(); + CommandScheduler.getInstance().run(); + long now = System.nanoTime(); + + if (driver.wasJustPressed(GamepadKeys.Button.A)) odometry.resetHeading(); + if (driver.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) shooter.toggle(); + if (driver.wasJustPressed(GamepadKeys.Button.DPAD_UP)) shooter.trim(Constants.SHOOTER_TRIM_STEP_RPM); + if (driver.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) shooter.trim(-Constants.SHOOTER_TRIM_STEP_RPM); + + // Where we are, how fast we are going, and where the Cell is. + Pose2D pose = odometry.getPose(); + double x = pose.getX(DistanceUnit.METER); + double y = pose.getY(DistanceUnit.METER); + double heading = pose.getHeading(AngleUnit.RADIANS); + double[] vel = odometry.getVelocity(); + tracker.recordPose(now, x, y, heading); + if (vision.hasTarget()) { + tracker.update(vision.getTargetFrameNanos(), vision.getTargetX(), vision.getTargetY(), vision.getTargetZ()); + } + + // Aim. Y held: solver heading if we know where the Cell is, else the raw tag bearing, else the stick. + boolean aiming = driver.isDown(GamepadKeys.Button.Y); + double rotate = driver.getRightX(); + ShotSolver.Shot shot = null; + if (aiming && tracker.hasTarget(now)) { + shot = solver.solve(x, y, heading, vel[0], vel[1], tracker.getX(), tracker.getY()); + shooter.setTargetRpm(shot.valid ? shot.rpm : shot.idleRpm); + rotate = aimRotation(Math.toDegrees(shot.headingErrorRad(heading))); + } else if (aiming && vision.hasTarget()) { + rotate = aimRotation(vision.getBearingDeg()); + } + lastShot = shot; + + boolean fieldCentric = !driver.isDown(GamepadKeys.Button.LEFT_BUMPER); + drive.drive(driver.getLeftY(), driver.getLeftX(), rotate, fieldCentric, heading); + + // Fire gate. With a solved shot: valid, at speed, and pointing inside the Cell width. + boolean fire = driver.isDown(GamepadKeys.Button.X); + boolean ready = shot == null + ? shooter.atSpeed() + : shot.valid && shooter.atSpeed() && Math.abs(shot.headingErrorRad(heading)) < shot.headingToleranceRad; + double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER); + double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); + boolean firing = false; + if (leftTrigger > Constants.TRIGGER_THRESHOLD) { + intake.reverse(); + indexer.reverse(); + } else if (rightTrigger > Constants.TRIGGER_THRESHOLD) { + intake.run(); + indexer.feed(); + } else if (fire && ready) { + intake.stop(); + indexer.feed(); + firing = true; + } else { + intake.stop(); + indexer.stop(); + } + + if (shot != null && log != null) { + log.row(x, y, heading, vel[0], vel[1], shot.distanceM, shot.radialVel, shot.tangentialVel, + shot.valid ? shot.rpm : shot.idleRpm, shooter.getLeftRpm(), shooter.getRightRpm(), + shot.headingErrorRad(heading), shot.valid ? 1 : 0, firing ? 1 : 0); + } + + telemetry.addData("Alliance", vision.getAlliance()); + telemetry.addData("Pose", "%.2f %.2f m %.0f deg v %.2f %.2f", x, y, Math.toDegrees(heading), vel[0], vel[1]); + telemetry.addData("Shooter", "%s target %.0f (trim %+.0f) L %.0f R %.0f %s", + shooter.isRunning() ? "ON" : "off", shooter.getTargetRpm(), shooter.getTrimRpm(), + shooter.getLeftRpm(), shooter.getRightRpm(), shooter.atSpeed() ? "AT SPEED" : ""); + telemetry.addData("Target", tracker.hasTarget(now) + ? String.format("%.2f %.2f m seen %.1f s ago", tracker.getX(), tracker.getY(), tracker.ageS(now)) + : vision.hasTarget() ? "tag only, no pose" : "none"); + telemetry.addData("Shot", shot == null ? "hold Y" : shot.valid + ? String.format("OK d %.2f radial %+.2f tang %+.2f rpm %.0f err %+.1f deg tol %.1f", + shot.distanceM, shot.radialVel, shot.tangentialVel, shot.rpm, + Math.toDegrees(shot.headingErrorRad(heading)), Math.toDegrees(shot.headingToleranceRad)) + : String.format("NO SHOT: %s d %.2f radial %+.2f", shot.reason, shot.distanceM, shot.radialVel)); + telemetry.addData("Log", log == null ? "not writing" : log.fileName()); + telemetry.addData("Battery", "%.1f V", battery.getVoltage()); + telemetry.update(); + } +``` + +`aimRotation(double bearingDeg)` from the teleop plan stays as it is: bearing positive left gives clockwise-negative rotation, which is the same sign as `headingErrorRad`. + +- [ ] **Step 4: Build and run all tests** + +```bash +./gradlew :TeamCode:assembleDebug :TeamCode:testDebugUnitTest --no-daemon +``` + +Expected: `BUILD SUCCESSFUL`. + +- [ ] **Step 5: Commit** + +```bash +git add TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +git commit -m "feature: solver-driven aim, fire gate, RPM trim and shot logging" +``` + +--- + +### Task 9: Tools README, measurement record and repo README + +**Files:** +- Create: `tools/shots/README.md` +- Modify: `README.md` + +**Interfaces:** +- Consumes: everything above. No code produced. + +- [ ] **Step 1: Write `tools/shots/README.md`** + +```markdown +# Shot table tools + +Lebob Robotics, FTC 29550 · BIOBUZZ 2026/27 · v1.0, 20 Sep 2026 + +The robot cannot change its launch angle, so whether a shot lands while +driving depends on distance and on how fast the robot is closing on the Cell. +These tools work that out on a laptop and give the robot a table. + +## Files + +- `shots.js`: the physics, the sweep and the Java export. The only copy. +- `index.html`: the visualiser. Open it in a browser (it fetches Plotly from + the internet the first time). Change inputs, press Compute, move the + sliders, export. +- `export.js`: `node tools/shots/export.js` regenerates + `TeamCode/.../ShotTable.java` from `inputs.json`. Run it after changing + `inputs.json` and commit both files together. +- `inputs.json`: the measured constants behind the committed table. + +## What the three panels show + +1. Trajectory fan. Every flywheel speed at the chosen distance and radial + velocity. Green arcs score, red miss, the blue one is what the robot will + use. The black segment is the Cell opening, the dotted line its front face. +2. RPM band against radial velocity at that distance. The green fill is the + set of speeds that score. Where it pinches shut is the closing speed the + driver cannot exceed. +3. Heatmap of the table. Grey cells have no shot. + +## Measurements + +Record here, then copy into `inputs.json` and regenerate. + +| Input | How to measure | Value | Date | Who | +| --- | --- | --- | --- | --- | +| `launchAngleDeg` | From the CAD, checked with a protractor on the exit guide | | | | +| `exitHeightM` | Tape from tiles to the ball centre at exit | | | | +| `exitSpeedPerRpm` | Fire Pollen at 3000, 4000, 5000 RPM from a fixed spot, slow-motion video past a metre rule, exit speed over RPM, fit a line through zero | | | | +| `dragCd` | Compare the landing distance of those shots to the page's prediction, adjust until they match. Start at 0.45 | | | | +| `shooterOffsetM` | Ball exit point forward of the robot's tracking point | | | | +| `openingTiltDeg`, `openingLengthM`, `lipHeightM` | Competition Manual Figure 9-10 and the field CAD | | | | +| `Constants.FEED_DELAY_S` | Video the indexer starting and the ball leaving, or count loops to the shooter current spike | | | | +| `Constants.CAMERA_*` | From the CAD once the mount exists | | | | + +Time of flight at two distances, from the same video, checked against the +table's `TOF_S` column. + +## Why the tolerance is 50 RPM + +With a fixed launch angle the near lip sets the lowest speed that scores and +the far edge of the 14 in opening sets the highest. With the current inputs +that window is about 0.3 m/s of ball speed, 125 to 175 RPM, at every distance +and for any launch angle from 45° to 75°. The table only keeps cells whose band +is at least twice `rpmToleranceRpm`, so the flywheel must hold ±50 RPM +(`Constants.SHOOTER_TOLERANCE_RPM`). Tune the shooter PIDF until the logged +`left_rpm` and `right_rpm` sit inside that at a steady target before spending +time on moving shots. A bigger window needs a hood, which is a hardware +conversation. + +## Reading the logs + +Each run writes `/sdcard/FIRST/shots/.csv` on the hub while Y is +held. Pull them with: + + adb pull /sdcard/FIRST/shots ./shots + +Columns are in `ShotLog.HEADER`. `valid` and `fired` are 0 or 1. A shot that +missed while `valid` was 1 and `heading_err_rad` was inside tolerance means the +table is off at that `dist_m` and `radial_mps`: check `exitSpeedPerRpm` and +`dragCd` first. +``` + +- [ ] **Step 2: Update the repo `README.md`** + +In the driver controls table added by the teleop plan, change the Y and X rows and add the D-pad row: + +```markdown +| Y (hold) | Aim: rotation tracks the solved shot heading, shooter runs at the table RPM. Falls back to tag bearing if the Cell position is unknown | +| X (hold) | Fire: feeds only when at speed, and when aiming also only with a valid shot and heading inside the Cell width | +| D-pad up / down | Trim every shooter target by ±50 RPM for the rest of the run | +``` + +After the on-robot checklist, add: + +```markdown +### Shoot on the move + +Tools and the measurement procedure are in `tools/shots/README.md`. Verify in +this order, recording hits in the PR: + +0. Velocity frame: strafe left with the robot facing +X and confirm the Pose + telemetry shows `v` growing in the second component, then turn 90° and + repeat; the same component must still grow. If it swaps, the Pinpoint + reports robot-frame velocity and `OdometrySubsystem.getVelocity()` must + rotate it by the heading before the solver sees it. +1. Stationary at 1.0, 1.5, 2.0 and 2.5 m: at least 8 of 10 Pollen in. Fix the + table inputs before moving on. +2. Strafing across the Cell at a steady speed: 7 of 10. +3. Driving toward and away at a steady speed: 7 of 10, and "NO SHOT: CLOSING + TOO FAST" appears near the speed the band chart predicts. +4. Free driving: log, video, count, fix what the log shows. +``` + +- [ ] **Step 3: Commit** + +```bash +git add tools/shots/README.md README.md +git commit -m "docs: shot table tools guide, measurement record and on-robot ladder" +``` + +--- + +## Testing on the robot + +Everything above compiles and passes JVM tests without a robot. What needs the robot, in order: + +1. **Rung 0, velocity frame.** Described in the README addition. Do this before any shooting; a frame mistake makes every moving shot wrong in a way that looks like a bad table. +2. **Measurements** from `tools/shots/README.md`, then `node tools/shots/export.js`, then commit `inputs.json` and `ShotTable.java` together. +3. **Rungs 1 to 4** from the README, each recorded in the PR under "Tested on the robot". +4. **Camera view while leading.** At rung 2, note on telemetry whether "Target" flips to "tag only" or "none" during the lead. If it expires mid-shot, the fix is a wider lens or a second camera, which is outside this plan. + +## After the plan + +- Pedro Pathing: `Robot` reads pose and velocity through `OdometrySubsystem`; when Pedro's localiser replaces it, keep those two getters and nothing else changes. +- Nectar: the runtime uses the Pollen table. A second generated table and a ball sensor would be a new spec. +- If logs show misses on direction changes, the next step is an acceleration term in `ShotSolver.solve`, not a bigger table. diff --git a/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md b/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md index 0efc2a6..34976cc 100644 --- a/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md +++ b/docs/superpowers/specs/2026-09-20-shoot-on-the-move-design.md @@ -105,6 +105,14 @@ flywheel tolerance. This is 4414's "most robust to errors" rule with speed as the only knob. No polynomial fit: the grid is 25 by 21 and the hub interpolates it directly. +Running the sweep with the placeholder inputs shows the band is 125 to 175 RPM +at every distance, for any launch angle from 45° to 75°: the near lip sets the +floor and the far edge of the 14 in opening sets the ceiling. Radial velocity +shifts the band rather than closing it, which is the point of putting it in +the table. The consequence is that the flywheel tolerance has to be 50 RPM, +not the teleop spec's 100, and holding that is a shooter tuning requirement +before any moving shot is attempted. + **Visualiser**, three panels matching the 4414 binder image: - Trajectory fan for the selected distance and radial velocity. Every RPM in the