From 1e93d12453570d486c93244e28e64602be2f4627 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:05:27 +0800 Subject: [PATCH 01/37] 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 02/37] 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 03/37] 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 a737e79d545f1aca5aaa1d293d6b224d6222807e Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:27:35 +0800 Subject: [PATCH 04/37] 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 3b9cfa8070e3f00108a9e14554b0f25babe225c5 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:32:41 +0800 Subject: [PATCH 05/37] feature: mecanum maths as a unit-tested pure function --- .github/workflows/build.yml | 3 ++ .../ftc/teamcode/MecanumKinematics.java | 40 ++++++++++++++++ .../subsystems/MecanumDriveSubsystem.java | 33 ++++--------- .../ftc/teamcode/MecanumKinematicsTest.java | 46 +++++++++++++++++++ 4 files changed, 98 insertions(+), 24 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumKinematics.java create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MecanumKinematicsTest.java diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ef09893..c945f84 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -50,3 +50,6 @@ jobs: - name: Assemble the TeamCode debug APK run: ./gradlew :TeamCode:assembleDebug --no-daemon --stacktrace + + - name: Run the TeamCode JVM unit tests + run: ./gradlew :TeamCode:testDebugUnitTest --no-daemon --stacktrace diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumKinematics.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumKinematics.java new file mode 100644 index 0000000..bfbeff5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumKinematics.java @@ -0,0 +1,40 @@ +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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java index 2f14fe6..f6ca42a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java @@ -4,7 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.MecanumKinematics; /** * Drivetrain subsystem for a four-motor mecanum base, with optional field-centric @@ -45,29 +45,14 @@ public MecanumDriveSubsystem(HardwareMap hardwareMap) { */ public void drive(double forward, double right, double rotate, boolean fieldCentric, double headingRadians) { if (fieldCentric) { - double theta = Math.atan2(forward, right); - double r = Math.hypot(right, forward); - - theta = AngleUnit.normalizeRadians(theta - headingRadians); - - forward = r * Math.sin(theta); - right = r * Math.cos(theta); + double[] rr = MecanumKinematics.toRobotRelative(forward, right, headingRadians); + forward = rr[0]; + right = rr[1]; } - - double frontLeftPower = forward + right + rotate; - double frontRightPower = forward - right - rotate; - double backLeftPower = forward - right + rotate; - double backRightPower = forward + right - rotate; - - double maxPower = 1.0; - maxPower = Math.max(maxPower, Math.abs(frontLeftPower)); - maxPower = Math.max(maxPower, Math.abs(frontRightPower)); - maxPower = Math.max(maxPower, Math.abs(backLeftPower)); - maxPower = Math.max(maxPower, Math.abs(backRightPower)); - - frontLeftDrive.setPower(frontLeftPower / maxPower); - frontRightDrive.setPower(frontRightPower / maxPower); - backLeftDrive.setPower(backLeftPower / maxPower); - backRightDrive.setPower(backRightPower / maxPower); + double[] p = MecanumKinematics.mix(forward, right, rotate); + frontLeftDrive.setPower(p[0]); + frontRightDrive.setPower(p[1]); + backLeftDrive.setPower(p[2]); + backRightDrive.setPower(p[3]); } } diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MecanumKinematicsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MecanumKinematicsTest.java new file mode 100644 index 0000000..e351c8d --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MecanumKinematicsTest.java @@ -0,0 +1,46 @@ +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); + } +} From 2030e2d367529367a57f2ea539e385e3d2f99a97 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:37:38 +0800 Subject: [PATCH 06/37] feature: constants class, hub bulk reads and GamepadEx controls --- .../firstinspires/ftc/teamcode/Alliance.java | 3 + .../firstinspires/ftc/teamcode/Constants.java | 57 +++++++++++++++++++ .../org/firstinspires/ftc/teamcode/Robot.java | 29 +++++----- .../subsystems/MecanumDriveSubsystem.java | 18 +++--- .../subsystems/OdometrySubsystem.java | 7 +-- 5 files changed, 88 insertions(+), 26 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java new file mode 100644 index 0000000..6a28949 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java @@ -0,0 +1,3 @@ +package org.firstinspires.ftc.teamcode; + +public enum Alliance { RED, BLUE } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java new file mode 100644 index 0000000..5844aef --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java @@ -0,0 +1,57 @@ +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; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 3b08345..a399794 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -1,6 +1,9 @@ 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; @@ -11,26 +14,24 @@ public class Robot { private final Telemetry telemetry; - private final Gamepad driver; + private final GamepadEx driver; public final MecanumDriveSubsystem drive; public final OdometrySubsystem odometry; - public Robot( - HardwareMap hardwareMap, - Telemetry telemetry, - Gamepad driver - ) { + public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) { this.telemetry = telemetry; - this.driver = driver; + 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(); - // Construct subsystems. drive = new MecanumDriveSubsystem(hardwareMap); odometry = new OdometrySubsystem(hardwareMap); - // intake = new IntakeSubsystem(hardwareMap); - // outtake = new OuttakeSubsystem(hardwareMap); } /** Called once when the OpMode enters INIT. */ @@ -40,15 +41,17 @@ public void init() { /** Called repeatedly while the OpMode is running. */ public void periodic() { + driver.readButtons(); CommandScheduler.getInstance().run(); - if (driver.a) { + if (driver.wasJustPressed(GamepadKeys.Button.A)) { odometry.resetHeading(); } // Hold left bumper to drive robot-relative; otherwise drive field-relative. - drive.drive(-driver.left_stick_y, driver.left_stick_x, driver.right_stick_x, - !driver.left_bumper, odometry.getPose().getHeading(AngleUnit.RADIANS)); + 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(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java index f6ca42a..93f12a7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.Constants; import org.firstinspires.ftc.teamcode.MecanumKinematics; /** @@ -17,16 +18,15 @@ public class MecanumDriveSubsystem extends SubsystemBase { private final DcMotor backRightDrive; public MecanumDriveSubsystem(HardwareMap hardwareMap) { - frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); - frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); - backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); - backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); + 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); - // Left motors are flipped so that a positive power on every motor drives straight. - frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); - frontRightDrive.setDirection(DcMotor.Direction.FORWARD); - backLeftDrive.setDirection(DcMotor.Direction.REVERSE); - backRightDrive.setDirection(DcMotor.Direction.FORWARD); + 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index aad8480..7de73c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -7,6 +7,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Constants; /** * Wraps a goBILDA Pinpoint Odometry Computer reading two dead-wheel pods, and @@ -16,14 +17,12 @@ public class OdometrySubsystem extends SubsystemBase { private final GoBildaPinpointDriver pinpoint; public OdometrySubsystem(HardwareMap hardwareMap) { - pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, Constants.PINPOINT); } /** Called once from the OpMode's init(). Configures the Pinpoint and zeroes its pose. */ public void init() { - // TODO: measure the pods' offsets from the robot's tracking point (usually its - // center) in mm, and set them here so Pinpoint reports an accurate field pose. - pinpoint.setOffsets(0.0, 0.0, DistanceUnit.MM); + pinpoint.setOffsets(Constants.PINPOINT_X_OFFSET_MM, Constants.PINPOINT_Y_OFFSET_MM, DistanceUnit.MM); pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); pinpoint.setEncoderDirections( GoBildaPinpointDriver.EncoderDirection.FORWARD, From 6a3b24d7ebc5531a97fc67a8aa1cbeddb192127e Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:41:37 +0800 Subject: [PATCH 07/37] feature: intake and indexer on the triggers --- .../org/firstinspires/ftc/teamcode/Robot.java | 19 +++++++++++ .../teamcode/subsystems/IndexerSubsystem.java | 32 +++++++++++++++++++ .../teamcode/subsystems/IntakeSubsystem.java | 32 +++++++++++++++++++ 3 files changed, 83 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index a399794..be91bd8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -9,6 +9,8 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.subsystems.IndexerSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem; import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem; import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; @@ -18,6 +20,8 @@ public class Robot { public final MecanumDriveSubsystem drive; public final OdometrySubsystem odometry; + public final IntakeSubsystem intake; + public final IndexerSubsystem indexer; public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) { this.telemetry = telemetry; @@ -32,6 +36,8 @@ public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad drive = new MecanumDriveSubsystem(hardwareMap); odometry = new OdometrySubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); + indexer = new IndexerSubsystem(hardwareMap); } /** Called once when the OpMode enters INIT. */ @@ -53,6 +59,19 @@ public void periodic() { drive.drive(driver.getLeftY(), driver.getLeftX(), driver.getRightX(), fieldCentric, odometry.getPose().getHeading(AngleUnit.RADIANS)); + 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(); + } + telemetry.addData("Pose", odometry.getPose()); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java new file mode 100644 index 0000000..fed5f8d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java @@ -0,0 +1,32 @@ +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); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..a4dd803 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java @@ -0,0 +1,32 @@ +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); + } +} From 9a8666dba8e07e25b419c6cb64372e6814293622 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:45:04 +0800 Subject: [PATCH 08/37] feature: shooter RPM conversions with tests --- .../ftc/teamcode/ShooterMath.java | 14 ++++++++++++++ .../ftc/teamcode/ShooterMathTest.java | 19 +++++++++++++++++++ 2 files changed, 33 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java new file mode 100644 index 0000000..1026f89 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterMath.java @@ -0,0 +1,14 @@ +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; + } +} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java new file mode 100644 index 0000000..3787782 --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShooterMathTest.java @@ -0,0 +1,19 @@ +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); + } +} From a479cb796aee6577d601da56b8c747386d7f4eb3 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:48:56 +0800 Subject: [PATCH 09/37] feature: closed-loop shooter with spin-up toggle and fire on X --- .../org/firstinspires/ftc/teamcode/Robot.java | 13 ++++ .../teamcode/subsystems/ShooterSubsystem.java | 64 +++++++++++++++++++ 2 files changed, 77 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index be91bd8..5cdfe53 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -13,6 +13,7 @@ import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem; import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem; import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; +import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem; public class Robot { private final Telemetry telemetry; @@ -22,6 +23,7 @@ public class Robot { public final OdometrySubsystem odometry; public final IntakeSubsystem intake; public final IndexerSubsystem indexer; + public final ShooterSubsystem shooter; public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) { this.telemetry = telemetry; @@ -38,6 +40,7 @@ public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad odometry = new OdometrySubsystem(hardwareMap); intake = new IntakeSubsystem(hardwareMap); indexer = new IndexerSubsystem(hardwareMap); + shooter = new ShooterSubsystem(hardwareMap); } /** Called once when the OpMode enters INIT. */ @@ -59,6 +62,11 @@ public void periodic() { drive.drive(driver.getLeftY(), driver.getLeftX(), driver.getRightX(), fieldCentric, odometry.getPose().getHeading(AngleUnit.RADIANS)); + if (driver.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { + shooter.toggle(); + } + boolean fire = driver.isDown(GamepadKeys.Button.X); + double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER); double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); if (leftTrigger > Constants.TRIGGER_THRESHOLD) { @@ -67,6 +75,10 @@ public void periodic() { } 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(); @@ -78,6 +90,7 @@ public void periodic() { /** Called once when the OpMode stops. */ public void stop() { + shooter.idle(); CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().reset(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..d2745e6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -0,0 +1,64 @@ +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); + } +} From a9c2a4fe82c90a09e47bcebf0eb7f8d6ab1f698c Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:53:26 +0800 Subject: [PATCH 10/37] feature: shooter idle coasts instead of braking --- .../ftc/teamcode/subsystems/ShooterSubsystem.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java index d2745e6..fd5a1f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -33,10 +33,11 @@ public void spinUp() { right.setVelocity(tps); } + /** Cuts power and lets the flywheels coast down (FLOAT), rather than braking to zero velocity. */ public void idle() { running = false; - left.setVelocity(0); - right.setVelocity(0); + left.setPower(0); + right.setPower(0); } public void toggle() { From 780d8af9e72b87ef221bbffa5ed5132c4f83cf52 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:56:54 +0800 Subject: [PATCH 11/37] feature: AprilTag aim assist on the alliance cell, alliance picked in init --- .../org/firstinspires/ftc/teamcode/Main.java | 10 +++ .../org/firstinspires/ftc/teamcode/Robot.java | 33 +++++++- .../teamcode/subsystems/VisionSubsystem.java | 77 +++++++++++++++++++ 3 files changed, 119 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java index 3a9752b..1764cc5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java @@ -13,6 +13,16 @@ public void init() { robot.init(); } + @Override + public void init_loop() { + robot.initLoop(); + } + + @Override + public void start() { + robot.start(); + } + @Override public void loop() { robot.periodic(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 5cdfe53..bf50991 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -14,6 +14,7 @@ import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem; import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.VisionSubsystem; public class Robot { private final Telemetry telemetry; @@ -24,6 +25,7 @@ public class Robot { public final IntakeSubsystem intake; public final IndexerSubsystem indexer; public final ShooterSubsystem shooter; + public final VisionSubsystem vision; public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) { this.telemetry = telemetry; @@ -41,6 +43,7 @@ public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad intake = new IntakeSubsystem(hardwareMap); indexer = new IndexerSubsystem(hardwareMap); shooter = new ShooterSubsystem(hardwareMap); + vision = new VisionSubsystem(hardwareMap); } /** Called once when the OpMode enters INIT. */ @@ -48,6 +51,22 @@ public void init() { odometry.init(); } + /** Called repeatedly while the OpMode sits in INIT. D-pad left = red, right = blue. */ + public void initLoop() { + driver.readButtons(); + CommandScheduler.getInstance().run(); + 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(); + } + /** Called repeatedly while the OpMode is running. */ public void periodic() { driver.readButtons(); @@ -59,7 +78,11 @@ public void periodic() { // 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(), + 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)); if (driver.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { @@ -91,7 +114,15 @@ public void periodic() { /** Called once when the OpMode stops. */ public void stop() { shooter.idle(); + vision.close(); CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().reset(); } + + /** 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)); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java new file mode 100644 index 0000000..c138048 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java @@ -0,0 +1,77 @@ +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(); + } +} From 521d5370b10e92e66c4234f19f1882538ff29ee3 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 12:03:00 +0800 Subject: [PATCH 12/37] docs: driver telemetry, config table, controls and on-robot checklist --- README.md | 58 +++++++++++++++++++ .../org/firstinspires/ftc/teamcode/Robot.java | 9 +++ 2 files changed, 67 insertions(+) diff --git a/README.md b/README.md index 6fd496f..1c313a6 100644 --- a/README.md +++ b/README.md @@ -114,3 +114,61 @@ run the `TeamCode` configuration. Android Studio does the same `adb install` and launch shown above. If the Hub does not appear in the device dropdown, run `adb connect 192.168.43.1:5555` in a terminal first; Android Studio picks up devices from the same adb server. + +## 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`. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index bf50991..e89d969 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -6,6 +6,7 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -19,6 +20,7 @@ public class Robot { private final Telemetry telemetry; private final GamepadEx driver; + private final VoltageSensor battery; public final MecanumDriveSubsystem drive; public final OdometrySubsystem odometry; @@ -35,6 +37,7 @@ public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad for (LynxModule hub : hardwareMap.getAll(LynxModule.class)) { hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } + battery = hardwareMap.voltageSensor.iterator().next(); CommandScheduler.getInstance().reset(); @@ -107,7 +110,13 @@ public void periodic() { indexer.stop(); } + 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(); } From 62747eb350f91fe5fbf00023114bb88cea280394 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 12:14:21 +0800 Subject: [PATCH 13/37] feature: shooter idle coasts, single velocity read per loop, robot drives without a camera --- .../org/firstinspires/ftc/teamcode/Robot.java | 1 + .../teamcode/subsystems/ShooterSubsystem.java | 33 +++++++++++++---- .../teamcode/subsystems/VisionSubsystem.java | 35 +++++++++++++------ 3 files changed, 51 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index e89d969..a8ce72c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -61,6 +61,7 @@ public void initLoop() { 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", vision.isAvailable() ? "ok" : "NOT FOUND"); telemetry.addData("Camera sees", vision.getTargetName()); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java index fd5a1f4..2978fa8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -13,6 +13,8 @@ public class ShooterSubsystem extends SubsystemBase { private final DcMotorEx left; private final DcMotorEx right; private boolean running; + private double leftRpm; + private double rightRpm; public ShooterSubsystem(HardwareMap hardwareMap) { left = hardwareMap.get(DcMotorEx.class, Constants.SHOOTER_LEFT); @@ -26,18 +28,28 @@ public ShooterSubsystem(HardwareMap hardwareMap) { } } + /** Back into RUN_USING_ENCODER so the hub PID holds the setpoint. */ public void spinUp() { running = true; double tps = ShooterMath.rpmToTicksPerSecond(Constants.SHOOTER_SETPOINT_RPM, Constants.SHOOTER_TICKS_PER_REV); + for (DcMotorEx m : new DcMotorEx[]{left, right}) { + m.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } left.setVelocity(tps); right.setVelocity(tps); } - /** Cuts power and lets the flywheels coast down (FLOAT), rather than braking to zero velocity. */ + /** + * Drops out of RUN_USING_ENCODER before cutting power, so the wheels coast down (FLOAT) + * instead of the hub PID braking them to zero velocity. spinUp() restores velocity control; + * the controller keeps PIDF coefficients per mode, so the constructor's gains survive. + */ public void idle() { running = false; - left.setPower(0); - right.setPower(0); + for (DcMotorEx m : new DcMotorEx[]{left, right}) { + m.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + m.setPower(0); + } } public void toggle() { @@ -48,18 +60,25 @@ public boolean isRunning() { return running; } + /** One velocity read per motor per loop; everything else reads the cache (bulk read stays at one). */ + @Override + public void periodic() { + leftRpm = ShooterMath.ticksPerSecondToRpm(left.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + rightRpm = ShooterMath.ticksPerSecondToRpm(right.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + } + /** 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; + && Math.abs(leftRpm - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM + && Math.abs(rightRpm - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM; } public double getLeftRpm() { - return ShooterMath.ticksPerSecondToRpm(left.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + return leftRpm; } public double getRightRpm() { - return ShooterMath.ticksPerSecondToRpm(right.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); + return rightRpm; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java index c138048..ba3f239 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java @@ -15,21 +15,33 @@ /** * 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". + * A missing or misconfigured webcam leaves the subsystem inert rather than killing the OpMode: + * isAvailable() is false, hasTarget() stays false, and aim assist simply does nothing. */ public class VisionSubsystem extends SubsystemBase { - private final AprilTagProcessor processor; - private final VisionPortal portal; + private AprilTagProcessor processor; + private 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(); + try { + processor = new AprilTagProcessor.Builder() + .setTagLibrary(AprilTagGameDatabase.getCurrentGameTagLibrary()) + .build(); + portal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, Constants.WEBCAM)) + .addProcessor(processor) + .build(); + } catch (RuntimeException e) { + processor = null; + portal = null; + } + } + + /** False when the webcam was missing or failed to open at construction. */ + public boolean isAvailable() { + return portal != null; } public void setAlliance(Alliance alliance) { @@ -43,6 +55,7 @@ public Alliance getAlliance() { /** Picks the best-seen cluster belonging to our alliance from the latest frame. */ @Override public void periodic() { + if (portal == null) return; AprilTagClusterDetection best = null; for (AprilTagDetection d : processor.getDetections()) { if (!(d instanceof AprilTagClusterDetection)) continue; @@ -68,10 +81,10 @@ public String getTargetName() { /** Turn off the Driver Station preview once the match starts to save CPU. */ public void stopLiveView() { - portal.stopLiveView(); + if (portal != null) portal.stopLiveView(); } public void close() { - portal.close(); + if (portal != null) portal.close(); } } From a9b7497129a642c6f74065d39171d9f682625ff6 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:25:52 +0800 Subject: [PATCH 14/37] 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 8b5d2dacfe7885f296a7cbd53363e996bb840865 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:26:14 +0800 Subject: [PATCH 15/37] 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 9264a2413f9c7f4689a49b7667c5487f7a79cefd Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 11:40:47 +0800 Subject: [PATCH 16/37] 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 From 3ff1356a1291902c4396eef90577ebaa809563c5 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 13:41:07 +0800 Subject: [PATCH 17/37] feature: shot ballistics module, node exporter and inputs file --- .../2026-09-20-shoot-on-the-move-design.md | 5 +- tools/shots/export.js | 21 ++ tools/shots/inputs.json | 16 ++ tools/shots/shots.js | 180 ++++++++++++++++++ 4 files changed, 221 insertions(+), 1 deletion(-) create mode 100644 tools/shots/export.js create mode 100644 tools/shots/inputs.json create mode 100644 tools/shots/shots.js 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 34976cc..4a8b0d2 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 @@ -67,7 +67,10 @@ Three parts, kept apart so each can be checked on its own. `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. A +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 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. diff --git a/tools/shots/export.js b/tools/shots/export.js new file mode 100644 index 0000000..404f1cf --- /dev/null +++ b/tools/shots/export.js @@ -0,0 +1,21 @@ +#!/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'); diff --git a/tools/shots/inputs.json b/tools/shots/inputs.json new file mode 100644 index 0000000..0b3e91a --- /dev/null +++ b/tools/shots/inputs.json @@ -0,0 +1,16 @@ +{ + "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 +} diff --git a/tools/shots/shots.js b/tools/shots/shots.js new file mode 100644 index 0000000..c2b8188 --- /dev/null +++ b/tools/shots/shots.js @@ -0,0 +1,180 @@ +// 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 }; +}); From 80539459beb612cab9ca4cd4966b73bcc8405238 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 13:41:49 +0800 Subject: [PATCH 18/37] feature: generated shot table from default inputs --- .../firstinspires/ftc/teamcode/ShotTable.java | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java new file mode 100644 index 0000000..4f045cd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java @@ -0,0 +1,107 @@ +package org.firstinspires.ftc.teamcode; + +/** + * GENERATED by tools/shots (node tools/shots/export.js). Do not edit by hand: change tools/shots/inputs.json + * and regenerate. Rows are distance to the near lip, columns are radial robot velocity (positive closing). + * Inputs: {"ball":"POLLEN","ballDiameterM":0.0711,"ballMassKg":0.0249,"dragCd":0.45,"launchAngleDeg":60,"exitHeightM":0.4,"exitSpeedPerRpm":0.0023,"shooterOffsetM":0.15,"rpmToleranceRpm":50,"lipHeightM":1.359,"openingWidthM":0.508,"openingLengthM":0.356,"openingTiltDeg":30,"marginM":0.02,"distMinM":0.6,"distMaxM":3,"distStepM":0.1,"velMinMps":-1,"velMaxMps":1,"velStepMps":0.1,"rpmMin":1500,"rpmMax":5500,"rpmStep":25,"dtS":0.002} + */ +public final class ShotTable { + private ShotTable() {} + + public static final String BALL = "POLLEN"; + public static final double LAUNCH_ANGLE_DEG = 60; + public static final double EXIT_HEIGHT_M = 0.4; + public static final double EXIT_SPEED_PER_RPM = 0.0023; + public static final double SHOOTER_OFFSET_M = 0.15; + public static final double OPENING_WIDTH_M = 0.508; + /** Ball radius plus edge margin: how far inside the opening's side edges the ball centre must pass. */ + public static final double LATERAL_CLEARANCE_M = 0.056; + public static final double MIN_BAND_RPM = 100; + + public static final double[] DIST_M = {0.6, 0.7, 0.8, 0.9, 1, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, 2.9, 3}; + public static final double[] VEL_MPS = {-1, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1}; + /** Band-centre flywheel RPM, [dist][vel]. NaN where no shot lands. */ + public static final double[][] RPM = { + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2400, 2375, 2350, 2350, 2350, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2437.5, 2400, 2400, 2375, 2362.5, 2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2475, 2450, 2425, 2400, 2387.5, 2375, 2362.5, 2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2550, 2500, 2475, 2450, 2425, 2400, 2387.5, 2375, 2362.5, 2350, 2337.5, 2350, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2600, 2550, 2525, 2500, 2475, 2450, 2425, 2400, 2387.5, 2375, 2362.5, 2362.5, 2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2650, 2612.5, 2575, 2550, 2512.5, 2487.5, 2462.5, 2437.5, 2425, 2400, 2387.5, 2375, 2362.5, 2362.5, 2362.5, 2350, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {2700, 2662.5, 2625, 2600, 2562.5, 2537.5, 2500, 2475, 2462.5, 2437.5, 2425, 2400, 2387.5, 2375, 2362.5, 2362.5, 2362.5, 2362.5, 2362.5, Double.NaN, Double.NaN}, + {2750, 2712.5, 2675, 2650, 2612.5, 2575, 2550, 2525, 2500, 2475, 2450, 2437.5, 2412.5, 2400, 2387.5, 2387.5, 2362.5, 2362.5, 2362.5, 2362.5, 2362.5}, + {2812.5, 2762.5, 2737.5, 2687.5, 2662.5, 2625, 2600, 2562.5, 2537.5, 2512.5, 2487.5, 2475, 2450, 2437.5, 2412.5, 2412.5, 2387.5, 2387.5, 2375, 2362.5, 2362.5}, + {2862.5, 2812.5, 2787.5, 2737.5, 2712.5, 2675, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2512.5, 2487.5, 2462.5, 2450, 2437.5, 2412.5, 2412.5, 2387.5, 2387.5, 2387.5}, + {2912.5, 2875, 2837.5, 2787.5, 2762.5, 2725, 2687.5, 2662.5, 2637.5, 2600, 2575, 2550, 2525, 2512.5, 2487.5, 2462.5, 2450, 2437.5, 2412.5, 2412.5, 2387.5}, + {2962.5, 2925, 2887.5, 2837.5, 2812.5, 2762.5, 2737.5, 2712.5, 2675, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2512.5, 2500, 2487.5, 2462.5, 2437.5, 2437.5, 2412.5}, + {3012.5, 2987.5, 2937.5, 2887.5, 2862.5, 2812.5, 2787.5, 2762.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2512.5, 2487.5, 2487.5, 2462.5, 2437.5}, + {3062.5, 3037.5, 2987.5, 2937.5, 2912.5, 2862.5, 2837.5, 2787.5, 2762.5, 2737.5, 2712.5, 2675, 2637.5, 2612.5, 2587.5, 2562.5, 2550, 2537.5, 2512.5, 2487.5, 2475}, + {3125, 3087.5, 3037.5, 2987.5, 2962.5, 2912.5, 2887.5, 2837.5, 2812.5, 2787.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2525, 2512.5}, + {3175, 3137.5, 3087.5, 3037.5, 3012.5, 2962.5, 2937.5, 2887.5, 2862.5, 2812.5, 2787.5, 2762.5, 2737.5, 2712.5, 2687.5, 2637.5, 2625, 2612.5, 2587.5, 2562.5, 2537.5}, + {3225, 3187.5, 3137.5, 3087.5, 3062.5, 3012.5, 2975, 2937.5, 2912.5, 2862.5, 2837.5, 2812.5, 2762.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5, 2587.5, 2562.5}, + {3275, 3237.5, 3187.5, 3137.5, 3100, 3062.5, 3025, 2987.5, 2950, 2912.5, 2887.5, 2837.5, 2812.5, 2787.5, 2762.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5}, + {3325, 3275, 3237.5, 3187.5, 3150, 3112.5, 3062.5, 3037.5, 2987.5, 2962.5, 2925, 2887.5, 2862.5, 2837.5, 2800, 2762.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5}, + {3375, 3325, 3287.5, 3237.5, 3200, 3162.5, 3112.5, 3075, 3037.5, 3012.5, 2962.5, 2937.5, 2900, 2862.5, 2837.5, 2812.5, 2787.5, 2762.5, 2737.5, 2712.5, 2687.5}, + {3425, 3375, 3337.5, 3287.5, 3250, 3200, 3162.5, 3125, 3087.5, 3050, 3012.5, 2987.5, 2950, 2912.5, 2887.5, 2850, 2825, 2787.5, 2762.5, 2737.5, 2712.5}, + {3475, 3425, 3375, 3337.5, 3300, 3250, 3212.5, 3175, 3137.5, 3100, 3062.5, 3025, 2987.5, 2962.5, 2925, 2887.5, 2862.5, 2837.5, 2812.5, 2787.5, 2762.5}, + {3525, 3475, 3425, 3387.5, 3337.5, 3300, 3262.5, 3212.5, 3175, 3137.5, 3100, 3062.5, 3037.5, 3000, 2962.5, 2937.5, 2912.5, 2875, 2850, 2812.5, 2787.5} + }; + /** Width of the valid RPM band, [dist][vel]. NaN where no shot lands. */ + public static final double[][] BAND_RPM = { + {75, 0, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {150, 125, 75, 25, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {150, 150, 150, 150, 100, 25, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {175, 150, 150, 150, 125, 150, 125, 50, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {150, 150, 150, 150, 125, 150, 125, 150, 125, 75, 0, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {150, 150, 150, 150, 150, 150, 125, 150, 125, 150, 125, 100, 25, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {150, 150, 150, 150, 150, 150, 150, 150, 125, 150, 125, 125, 150, 125, 50, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {150, 125, 150, 150, 125, 125, 125, 125, 150, 150, 125, 150, 125, 125, 125, 100, 75, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {150, 125, 150, 150, 125, 125, 150, 150, 125, 125, 150, 150, 125, 150, 125, 125, 125, 125, 125, 25, Double.NaN}, + {150, 125, 150, 150, 125, 150, 150, 150, 150, 150, 150, 125, 125, 150, 125, 125, 125, 125, 125, 125, 125}, + {125, 125, 125, 125, 125, 150, 150, 125, 125, 125, 125, 150, 150, 125, 125, 125, 125, 125, 150, 125, 125}, + {125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125, 125}, + {125, 150, 125, 125, 125, 150, 125, 125, 125, 150, 150, 150, 150, 125, 125, 125, 150, 125, 125, 125, 125}, + {125, 150, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125}, + {125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125}, + {125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 150, 125, 125, 125, 150}, + {100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125}, + {100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125}, + {100, 125, 125, 125, 125, 125, 100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125}, + {100, 125, 125, 125, 100, 125, 100, 125, 100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125}, + {100, 100, 125, 125, 100, 125, 125, 125, 125, 125, 100, 125, 125, 125, 100, 125, 125, 125, 125, 125, 125}, + {100, 100, 125, 125, 100, 125, 125, 100, 125, 125, 125, 125, 100, 125, 125, 125, 125, 125, 125, 125, 125}, + {100, 100, 125, 125, 100, 100, 125, 100, 125, 100, 125, 125, 100, 125, 125, 100, 100, 125, 125, 125, 125}, + {100, 100, 100, 125, 100, 100, 125, 100, 125, 100, 125, 100, 125, 125, 100, 125, 125, 125, 125, 125, 125}, + {100, 100, 100, 125, 125, 100, 125, 125, 100, 125, 100, 125, 125, 100, 125, 125, 125, 100, 100, 125, 125} + }; + /** Time of flight at the band centre, seconds, [dist][vel]. */ + public static final double[][] TOF_S = { + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.518, 0.492, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.572, 0.546, 0.518, 0.5, 0.48, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.612, 0.584, 0.568, 0.544, 0.522, 0.5, 0.476, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.648, 0.626, 0.604, 0.58, 0.562, 0.542, 0.522, 0.502, 0.48, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.69, 0.662, 0.642, 0.62, 0.6, 0.578, 0.56, 0.54, 0.522, 0.502, 0.482, 0.47, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.724, 0.698, 0.678, 0.658, 0.636, 0.616, 0.596, 0.574, 0.558, 0.54, 0.522, 0.506, 0.488, 0.468, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.754, 0.732, 0.71, 0.69, 0.668, 0.648, 0.628, 0.608, 0.592, 0.572, 0.556, 0.538, 0.52, 0.508, 0.494, 0.476, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {0.784, 0.762, 0.742, 0.722, 0.7, 0.682, 0.66, 0.64, 0.624, 0.606, 0.59, 0.57, 0.554, 0.538, 0.52, 0.508, 0.494, 0.482, 0.468, Double.NaN, Double.NaN}, + {0.812, 0.792, 0.77, 0.752, 0.73, 0.71, 0.692, 0.672, 0.654, 0.636, 0.618, 0.602, 0.584, 0.568, 0.552, 0.54, 0.52, 0.508, 0.496, 0.484, 0.472}, + {0.844, 0.82, 0.802, 0.778, 0.76, 0.74, 0.722, 0.7, 0.682, 0.664, 0.646, 0.632, 0.614, 0.598, 0.58, 0.568, 0.55, 0.538, 0.524, 0.508, 0.498}, + {0.87, 0.846, 0.828, 0.806, 0.788, 0.768, 0.746, 0.73, 0.712, 0.694, 0.678, 0.66, 0.642, 0.624, 0.61, 0.596, 0.578, 0.568, 0.55, 0.538, 0.528}, + {0.894, 0.874, 0.854, 0.832, 0.814, 0.794, 0.774, 0.758, 0.74, 0.72, 0.704, 0.686, 0.67, 0.656, 0.638, 0.622, 0.608, 0.594, 0.576, 0.566, 0.548}, + {0.92, 0.9, 0.88, 0.858, 0.84, 0.818, 0.802, 0.784, 0.764, 0.744, 0.728, 0.712, 0.694, 0.678, 0.662, 0.648, 0.634, 0.618, 0.602, 0.59, 0.574}, + {0.944, 0.926, 0.904, 0.882, 0.866, 0.844, 0.826, 0.81, 0.788, 0.772, 0.756, 0.738, 0.722, 0.706, 0.69, 0.674, 0.658, 0.642, 0.63, 0.614, 0.598}, + {0.966, 0.95, 0.928, 0.906, 0.89, 0.868, 0.852, 0.83, 0.814, 0.798, 0.782, 0.762, 0.744, 0.728, 0.712, 0.696, 0.682, 0.67, 0.654, 0.638, 0.624}, + {0.992, 0.974, 0.952, 0.93, 0.914, 0.892, 0.876, 0.854, 0.838, 0.822, 0.8, 0.784, 0.768, 0.754, 0.738, 0.722, 0.706, 0.69, 0.674, 0.662, 0.65}, + {1.014, 0.996, 0.974, 0.952, 0.938, 0.916, 0.9, 0.878, 0.862, 0.842, 0.826, 0.81, 0.794, 0.778, 0.762, 0.742, 0.73, 0.716, 0.702, 0.686, 0.67}, + {1.036, 1.018, 0.996, 0.976, 0.96, 0.938, 0.92, 0.902, 0.886, 0.864, 0.85, 0.834, 0.812, 0.798, 0.782, 0.766, 0.752, 0.736, 0.722, 0.706, 0.692}, + {1.058, 1.04, 1.018, 0.998, 0.98, 0.962, 0.942, 0.924, 0.906, 0.888, 0.872, 0.852, 0.836, 0.822, 0.806, 0.792, 0.776, 0.76, 0.746, 0.732, 0.716}, + {1.08, 1.058, 1.04, 1.02, 1.002, 0.984, 0.962, 0.946, 0.926, 0.91, 0.892, 0.874, 0.86, 0.844, 0.828, 0.81, 0.794, 0.78, 0.764, 0.75, 0.736}, + {1.1, 1.08, 1.062, 1.04, 1.022, 1.004, 0.984, 0.966, 0.948, 0.934, 0.912, 0.898, 0.88, 0.862, 0.848, 0.832, 0.818, 0.804, 0.788, 0.774, 0.76}, + {1.12, 1.1, 1.082, 1.062, 1.044, 1.024, 1.006, 0.988, 0.97, 0.952, 0.934, 0.92, 0.902, 0.884, 0.87, 0.852, 0.838, 0.82, 0.806, 0.792, 0.778}, + {1.142, 1.12, 1.1, 1.082, 1.064, 1.044, 1.026, 1.01, 0.992, 0.974, 0.956, 0.938, 0.922, 0.906, 0.89, 0.872, 0.858, 0.844, 0.828, 0.814, 0.8}, + {1.162, 1.14, 1.12, 1.102, 1.082, 1.066, 1.048, 1.028, 1.01, 0.992, 0.976, 0.958, 0.942, 0.926, 0.908, 0.894, 0.88, 0.862, 0.848, 0.832, 0.818} + }; +} From ce78abc3952269bef56990082e8bc30957ccd8f2 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 13:42:42 +0800 Subject: [PATCH 19/37] feature: shot table visualiser with trajectory fan, band chart and heatmap --- tools/shots/index.html | 181 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 181 insertions(+) create mode 100644 tools/shots/index.html diff --git a/tools/shots/index.html b/tools/shots/index.html new file mode 100644 index 0000000..839e203 --- /dev/null +++ b/tools/shots/index.html @@ -0,0 +1,181 @@ + + + + +BIOBUZZ shot table + + + + + +
+

Ball

+ + +

Actions

+ + + + +

+
+
+ +
+ + + + +
+
+
+
+
+ + + From 81a35a1f6da6e3c655cb30b108f9397b261f2532 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 13:46:32 +0800 Subject: [PATCH 20/37] feature: shot solver with table lookup and heading lead, tested --- .../ftc/teamcode/ShotSolver.java | 151 +++++++++++++++++ .../ftc/teamcode/ShotSolverTest.java | 153 ++++++++++++++++++ 2 files changed, 304 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java new file mode 100644 index 0000000..378eeb7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java @@ -0,0 +1,151 @@ +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; + } +} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java new file mode 100644 index 0000000..e1ab01f --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java @@ -0,0 +1,153 @@ +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); + } +} From 0ca1fee9d14fce440217bc3fa7ab580cf8d51ea3 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 13:47:12 +0800 Subject: [PATCH 21/37] feature: target tracker placing the cell from timestamped detections, tested --- .../ftc/teamcode/TargetTracker.java | 110 ++++++++++++++++++ .../ftc/teamcode/TargetTrackerTest.java | 105 +++++++++++++++++ 2 files changed, 215 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java new file mode 100644 index 0000000..7ce3db4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java @@ -0,0 +1,110 @@ +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; + } +} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java new file mode 100644 index 0000000..b653554 --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java @@ -0,0 +1,105 @@ +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)); + } +} From e7707e9f5c91aa2b513b92e64d275d3f34e2ef38 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 13:57:03 +0800 Subject: [PATCH 22/37] feature: visualiser panels sized to their grid tracks --- tools/shots/index.html | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/tools/shots/index.html b/tools/shots/index.html index 839e203..3827402 100644 --- a/tools/shots/index.html +++ b/tools/shots/index.html @@ -6,18 +6,22 @@ @@ -137,7 +141,7 @@

Actions

traces.push({ x: [o.x0, o.x0], y: [0, o.y0], mode: 'lines', name: 'Cell front', line: { color: '#000', width: 2, dash: 'dot' } }); Plotly.react('fan', traces, { title: 'Shots at ' + d.toFixed(2) + ' m, radial ' + vr.toFixed(2) + ' m/s' + (Number.isNaN(chosen) ? ' (NO SHOT)' : ''), - xaxis: { title: 'distance (m)', range: [0, Math.max(3.5, o.x1 + 0.5)] }, yaxis: { title: 'height (m)', scaleanchor: 'x', range: [0, 3] }, + xaxis: { title: 'distance (m)', range: [0, Math.max(3.5, o.x1 + 0.5)] }, yaxis: { title: 'height (m)', range: [0, 3.2] }, margin: { t: 40, r: 10 }, showlegend: false, annotations: [{ x: 0, y: params.exitHeightM, ax: -vr * 60, ay: 0, xref: 'x', yref: 'y', axref: 'pixel', ayref: 'pixel', showarrow: true, arrowhead: 2, text: 'robot ' + vr.toFixed(1) + ' m/s' }], }, { responsive: true }); @@ -151,7 +155,7 @@

Actions

{ x: x, y: grid.lo[i], mode: 'lines', name: 'lowest scoring RPM', line: { color: 'red' }, fill: 'tonexty', fillcolor: 'rgba(0,160,60,0.15)' }, { x: x, y: grid.rpm[i], mode: 'lines+markers', name: 'table RPM', line: { color: '#1f5fbf', width: 3 } }, { x: [grid.vel[+$('velIdx').value]], y: [grid.rpm[i][+$('velIdx').value]], mode: 'markers', name: 'selected', marker: { color: '#1f5fbf', size: 12, symbol: 'star' } }, - ], { title: 'Valid RPM band at ' + d.toFixed(2) + ' m', xaxis: { title: 'radial velocity (m/s, + closing)' }, yaxis: { title: 'flywheel RPM' }, margin: { t: 40, r: 10 }, legend: { orientation: 'h', y: -0.25 } }, { responsive: true }); + ], { title: 'Valid RPM band at ' + d.toFixed(2) + ' m', xaxis: { title: 'radial velocity (m/s, + closing)' }, yaxis: { title: 'flywheel RPM' }, margin: { t: 40, r: 10 }, legend: { orientation: 'h', y: 1.12 } }, { responsive: true }); } // Panel 3: the whole table. Blank cells have no shot. From 5d6c5fe555fe6b626d85bf9666e3078029219b85 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:03:38 +0800 Subject: [PATCH 23/37] feature: shot solver rejects cells with no time of flight or no horizontal exit speed --- .../ftc/teamcode/ShotSolver.java | 11 ++++++--- .../ftc/teamcode/ShotSolverTest.java | 23 +++++++++++++++++++ 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java index 378eeb7..f85ff21 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java @@ -106,16 +106,21 @@ public Shot solve(double x, double y, double headingRad, double vx, double vy, d double idle = Double.isNaN(stationary) ? fallbackRpm : stationary; double tolerance = Math.atan(halfOpeningM / d); + // 5. Horizontal exit speed, needed both to gate the shot and (below) to compute the lead. + // cos(90 deg) in doubles is ~6e-17, not exactly zero, so an unguarded ratio would clamp to a + // bogus +-90 deg lead instead of rejecting the shot; the epsilon below catches that and NaN. + double horizontalExit = tableRpm * exitSpeedPerRpm * Math.cos(launchAngleRad); + 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"; + else if (Double.isNaN(tableRpm) || Double.isNaN(tableBand) || Double.isNaN(tableTof) + || tableBand < minBandRpm || !(horizontalExit > 1e-6)) 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); + // 6. Heading lead so exit velocity plus robot velocity points along the bearing. 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); } diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java index e1ab01f..1deb91a 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java @@ -123,6 +123,29 @@ public void narrowBandIsInvalid() { assertEquals("NO SHOT", s.reason); } + @Test + public void missingTimeOfFlightIsInvalid() { + // Same RPM/BAND fixture, but the TOF cells around (2 m, 0 m/s) are missing. + double[][] tofMissing = { + {0.8, 0.8, 0.8}, + {Double.NaN, Double.NaN, Double.NaN}, + {1.2, 1.2, 1.2}}; + ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, tofMissing, 60, 0.002, 0, 0, 200, 0.508, 0.0555, 3500); + ShotSolver.Shot shot = s.solve(0, 0, 0, 0, 0, 2, 0); + assertFalse(shot.valid); + assertEquals("NO SHOT", shot.reason); + } + + @Test + public void zeroHorizontalExitSpeedIsInvalid() { + // 90 deg launch: horizontal exit speed is ~0, so no tangential lead is possible. + ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, TOF, 90, 0.002, 0, 0, 200, 0.508, 0.0555, 3500); + ShotSolver.Shot shot = s.solve(0, 0, 0, 0, 0.5, 2, 0); + assertFalse(shot.valid); + assertEquals("NO SHOT", shot.reason); + assertFalse(Double.isNaN(shot.headingRad)); + } + @Test public void feedDelayAdvancesThePose() { ShotSolver.Shot s = solver(0, 0.5).solve(0, 0, 0, 1.0, 0, 2, 0); From 6d688bae8863afef57e54cdb9c46d57006e42344 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:06:10 +0800 Subject: [PATCH 24/37] feature: adjustable shooter target with trim, odometry velocity, vision target position --- .../firstinspires/ftc/teamcode/Constants.java | 16 ++++++++- .../subsystems/OdometrySubsystem.java | 5 +++ .../teamcode/subsystems/ShooterSubsystem.java | 33 ++++++++++++++++--- .../teamcode/subsystems/VisionSubsystem.java | 23 +++++++++++++ 4 files changed, 72 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java index 5844aef..42d3859 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java @@ -40,7 +40,7 @@ private Constants() {} // 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; + public static final double SHOOTER_TOLERANCE_RPM = 50.0; // the shot table needs the wheel within this; see tools/shots/README.md // 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; @@ -54,4 +54,18 @@ private Constants() {} 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; + + // 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"; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index 7de73c1..f7914e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -41,6 +41,11 @@ public Pose2D getPose() { return pinpoint.getPosition(); } + /** 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)}; + } + /** Zeroes the reported heading in place, keeping the current position. */ public void resetHeading() { Pose2D current = pinpoint.getPosition(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java index 2978fa8..3928b23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -15,6 +15,8 @@ public class ShooterSubsystem extends SubsystemBase { private boolean running; private double leftRpm; private double rightRpm; + private double targetRpm = Constants.SHOOTER_SETPOINT_RPM; + private double trimRpm = 0; public ShooterSubsystem(HardwareMap hardwareMap) { left = hardwareMap.get(DcMotorEx.class, Constants.SHOOTER_LEFT); @@ -31,7 +33,7 @@ public ShooterSubsystem(HardwareMap hardwareMap) { /** Back into RUN_USING_ENCODER so the hub PID holds the setpoint. */ public void spinUp() { running = true; - double tps = ShooterMath.rpmToTicksPerSecond(Constants.SHOOTER_SETPOINT_RPM, Constants.SHOOTER_TICKS_PER_REV); + double tps = ShooterMath.rpmToTicksPerSecond(getTargetRpm(), Constants.SHOOTER_TICKS_PER_REV); for (DcMotorEx m : new DcMotorEx[]{left, right}) { m.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } @@ -39,6 +41,28 @@ public void spinUp() { 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; + } + /** * Drops out of RUN_USING_ENCODER before cutting power, so the wheels coast down (FLOAT) * instead of the hub PID braking them to zero velocity. spinUp() restores velocity control; @@ -67,11 +91,12 @@ public void periodic() { rightRpm = ShooterMath.ticksPerSecondToRpm(right.getVelocity(), Constants.SHOOTER_TICKS_PER_REV); } - /** True when both wheels are within tolerance of the setpoint. */ + /** True when both wheels are within tolerance of the current target. */ public boolean atSpeed() { + double target = getTargetRpm(); return running - && Math.abs(leftRpm - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM - && Math.abs(rightRpm - Constants.SHOOTER_SETPOINT_RPM) < Constants.SHOOTER_TOLERANCE_RPM; + && Math.abs(leftRpm - target) < Constants.SHOOTER_TOLERANCE_RPM + && Math.abs(rightRpm - target) < Constants.SHOOTER_TOLERANCE_RPM; } public double getLeftRpm() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java index ba3f239..815f0a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java @@ -4,6 +4,8 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Constants; import org.firstinspires.ftc.vision.VisionPortal; @@ -28,6 +30,7 @@ public VisionSubsystem(HardwareMap hardwareMap) { try { processor = new AprilTagProcessor.Builder() .setTagLibrary(AprilTagGameDatabase.getCurrentGameTagLibrary()) + .setOutputUnits(DistanceUnit.METER, AngleUnit.DEGREES) .build(); portal = new VisionPortal.Builder() .setCamera(hardwareMap.get(WebcamName.class, Constants.WEBCAM)) @@ -75,6 +78,26 @@ public double getBearingDeg() { return target.ftcPose.bearing; } + /** 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; + } + public String getTargetName() { return target == null ? "none" : target.metadata.name; } From b6c9d41e412e91d76d4261acd0ad98e5d3d9c356 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:06:16 +0800 Subject: [PATCH 25/37] feature: CSV shot log on the hub --- .../firstinspires/ftc/teamcode/ShotLog.java | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotLog.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotLog.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotLog.java new file mode 100644 index 0000000..85b2f17 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotLog.java @@ -0,0 +1,73 @@ +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; + } +} From 391cc338dc0f7fb273d4826f20f2ccaf604b97bd Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:10:25 +0800 Subject: [PATCH 26/37] feature: solver-driven aim, fire gate, RPM trim and shot logging --- .../org/firstinspires/ftc/teamcode/Robot.java | 84 +++++++++++++++---- 1 file changed, 66 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index a8ce72c..03ac1ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -10,6 +10,8 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.teamcode.subsystems.IndexerSubsystem; import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem; import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem; @@ -29,6 +31,16 @@ public class Robot { public final ShooterSubsystem shooter; public final VisionSubsystem vision; + 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; + public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) { this.telemetry = telemetry; this.driver = new GamepadEx(driverGamepad); @@ -69,60 +81,96 @@ public void initLoop() { /** Called once when the driver presses START. */ public void start() { vision.stopLiveView(); + log = ShotLog.open(Constants.SHOT_LOG_DIR); } /** Called repeatedly while the OpMode is running. */ public void periodic() { driver.readButtons(); CommandScheduler.getInstance().run(); - - if (driver.wasJustPressed(GamepadKeys.Button.A)) { - odometry.resetHeading(); + 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()); } - // Hold left bumper to drive robot-relative; otherwise drive field-relative. - boolean fieldCentric = !driver.isDown(GamepadKeys.Button.LEFT_BUMPER); + // 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(); - if (driver.isDown(GamepadKeys.Button.Y) && vision.hasTarget()) { + 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()); } - drive.drive(driver.getLeftY(), driver.getLeftX(), rotate, - fieldCentric, odometry.getPose().getHeading(AngleUnit.RADIANS)); - if (driver.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { - shooter.toggle(); - } - boolean fire = driver.isDown(GamepadKeys.Button.X); + 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 && shooter.atSpeed()) { - // Fire: feed only while the flywheels are at speed so every shot leaves at the same velocity. + } 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", odometry.getPose()); - telemetry.addData("Shooter", "%s target %.0f L %.0f R %.0f %s", - shooter.isRunning() ? "ON" : "off", Constants.SHOOTER_SETPOINT_RPM, + 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("Tag bearing", vision.hasTarget() ? String.format("%.1f deg", vision.getBearingDeg()) : "no target"); + 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(); } /** Called once when the OpMode stops. */ public void stop() { + if (log != null) log.close(); shooter.idle(); vision.close(); CommandScheduler.getInstance().cancelAll(); From a3a94c25c143f2e6c80c42629b981b426e271c69 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:12:05 +0800 Subject: [PATCH 27/37] docs: shot table tools guide, measurement record and on-robot ladder --- README.md | 22 ++++++++++++-- tools/shots/README.md | 70 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 90 insertions(+), 2 deletions(-) create mode 100644 tools/shots/README.md diff --git a/README.md b/README.md index 1c313a6..0cffc30 100644 --- a/README.md +++ b/README.md @@ -149,8 +149,9 @@ or right (blue). | 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 | +| X (hold) | Fire: feeds only when at speed, and when aiming also only with a valid shot and heading inside the Cell width | +| 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 | +| D-pad up / down | Trim every shooter target by ±50 RPM for the rest of the run | ## Testing on the robot @@ -172,3 +173,20 @@ result in the PR. Tuning values (shooter RPM and PIDF, aim gain, Pinpoint pod offsets) live in `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java`. + +### 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. diff --git a/tools/shots/README.md b/tools/shots/README.md new file mode 100644 index 0000000..e385136 --- /dev/null +++ b/tools/shots/README.md @@ -0,0 +1,70 @@ +# 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. From 253821fd92dcdf76d4f4fb3a6998228b75f9de3f Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:18:06 +0800 Subject: [PATCH 28/37] feature: heading reset clears the tracked target --- .../org/firstinspires/ftc/teamcode/Robot.java | 5 ++++- .../ftc/teamcode/TargetTracker.java | 7 +++++++ .../ftc/teamcode/TargetTrackerTest.java | 16 ++++++++++++++++ 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 03ac1ba..0287ddf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -90,7 +90,10 @@ public void periodic() { CommandScheduler.getInstance().run(); long now = System.nanoTime(); - if (driver.wasJustPressed(GamepadKeys.Button.A)) odometry.resetHeading(); + if (driver.wasJustPressed(GamepadKeys.Button.A)) { + odometry.resetHeading(); + tracker.clear(); // the reset rotates the field frame; the old target and poses no longer apply + } 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java index 7ce3db4..6b25fde 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java @@ -107,4 +107,11 @@ public double getY() { public double ageS(long nowNanos) { return seen ? (nowNanos - seenNanos) / 1e9 : Double.POSITIVE_INFINITY; } + + /** Forgets the target and every recorded pose. Call whenever the field frame moves, e.g. a heading reset. */ + public void clear() { + seen = false; + count = 0; + next = 0; + } } diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java index b653554..f2f4bb7 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java @@ -102,4 +102,20 @@ public void expiresAfterConfiguredAge() { assertEquals(4, t.ageS(4 * S), EPS); assertFalse(t.hasTarget(6 * S)); } + + @Test + public void clearForgetsTargetAndPoses() { + 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(0)); + + t.clear(); + + assertFalse(t.hasTarget(0)); + assertFalse(t.update(0, 0, 2, 0)); // no pose since clear(): same as never having recorded one + + t.recordPose(S, 0, 0, 0); + assertTrue(t.update(0, 0, 2, 0)); // recovers once a fresh pose lands + } } From 76e1f60f549a84a47fc481158bb3d685d0312e20 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:19:19 +0800 Subject: [PATCH 29/37] feature: shooter retargets only on a 10 RPM change and without re-asserting run mode --- .../firstinspires/ftc/teamcode/Constants.java | 2 ++ .../teamcode/subsystems/ShooterSubsystem.java | 19 ++++++++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java index 42d3859..0ac078b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java @@ -41,6 +41,8 @@ private Constants() {} 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 = 50.0; // the shot table needs the wheel within this; see tools/shots/README.md + // The scoring band is about 125 RPM wide, so a target change under this is noise: skip the hub writes. + public static final double SHOOTER_RETARGET_RPM = 10.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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java index 3928b23..201eaf6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -33,19 +33,28 @@ public ShooterSubsystem(HardwareMap hardwareMap) { /** Back into RUN_USING_ENCODER so the hub PID holds the setpoint. */ public void spinUp() { running = true; - double tps = ShooterMath.rpmToTicksPerSecond(getTargetRpm(), Constants.SHOOTER_TICKS_PER_REV); for (DcMotorEx m : new DcMotorEx[]{left, right}) { m.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } + sendVelocity(); + } + + /** Sends the current target to both motors without touching the run mode. */ + private void sendVelocity() { + 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. */ + /** + * Changes the target. Ignores moves smaller than Constants.SHOOTER_RETARGET_RPM (noise next to the + * scoring band) and, when running, only re-sends the velocity: re-asserting RUN_USING_ENCODER every + * loop would restart the hub PID's settling instead of letting it hold. + */ public void setTargetRpm(double rpm) { - if (rpm == targetRpm) return; + if (Math.abs(rpm - targetRpm) < Constants.SHOOTER_RETARGET_RPM) return; targetRpm = rpm; - if (running) spinUp(); + if (running) sendVelocity(); } /** Target plus the driver's trim. */ @@ -56,7 +65,7 @@ public double getTargetRpm() { /** Driver adjustment applied to every target for the rest of the run. */ public void trim(double deltaRpm) { trimRpm += deltaRpm; - if (running) spinUp(); + if (running) sendVelocity(); } public double getTrimRpm() { From fdbe31e1ab1415209a8f28f94c2969873b04dab4 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:23:43 +0800 Subject: [PATCH 30/37] feature: shot distance measured to the near lip, matching the table --- .../org/firstinspires/ftc/teamcode/Robot.java | 2 +- .../firstinspires/ftc/teamcode/ShotSolver.java | 14 ++++++++++---- .../firstinspires/ftc/teamcode/ShotTable.java | 2 ++ .../ftc/teamcode/ShotSolverTest.java | 17 +++++++++++++---- tools/shots/README.md | 5 +++++ tools/shots/shots.js | 4 +++- 6 files changed, 34 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 0287ddf..e879c0d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -33,7 +33,7 @@ public class Robot { 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, + ShotTable.LAUNCH_ANGLE_DEG, ShotTable.EXIT_SPEED_PER_RPM, ShotTable.SHOOTER_OFFSET_M, ShotTable.LIP_TO_CENTRE_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( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java index f85ff21..a3f5256 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java @@ -20,6 +20,7 @@ public static final class Shot { public final double headingRad; /** Heading error allowed by the Cell width at this distance, radians. */ public final double headingToleranceRad; + /** Distance to the Cell's near lip, matching the table's distance axis (targetX/targetY is the opening centre). */ public final double distanceM; public final double radialVel; public final double tangentialVel; @@ -55,14 +56,16 @@ public double headingErrorRad(double currentHeadingRad) { private final double launchAngleRad; private final double exitSpeedPerRpm; private final double shooterOffsetM; + private final double lipToCentreM; 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) { + double launchAngleDeg, double exitSpeedPerRpm, double shooterOffsetM, double lipToCentreM, + double feedDelayS, double minBandRpm, double openingWidthM, double lateralClearanceM, + double fallbackRpm) { this.dist = dist; this.vel = vel; this.rpm = rpm; @@ -71,6 +74,7 @@ public ShotSolver(double[] dist, double[] vel, double[][] rpm, double[][] band, this.launchAngleRad = Math.toRadians(launchAngleDeg); this.exitSpeedPerRpm = exitSpeedPerRpm; this.shooterOffsetM = shooterOffsetM; + this.lipToCentreM = lipToCentreM; this.feedDelayS = feedDelayS; this.minBandRpm = minBandRpm; this.halfOpeningM = openingWidthM / 2 - lateralClearanceM; @@ -80,7 +84,7 @@ public ShotSolver(double[] dist, double[] vel, double[][] rpm, double[][] band, /** * @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 + * @param targetX, targetY Cell opening centre, field frame (what TargetTracker reports) */ 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. @@ -93,11 +97,13 @@ public Shot solve(double x, double y, double headingRad, double vx, double vy, d 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. + // 3. Robot velocity along and across the line to the Cell, from the un-shortened distance. double ux = dx / d; double uy = dy / d; double radial = vx * ux + vy * uy; double tangential = -vx * uy + vy * ux; + // The table's distance axis is to the Cell's near lip; the Cell tag gives the opening centre. + d -= lipToCentreM; // 4. Table lookup. double tableRpm = interpolate(dist, vel, rpm, d, radial); double tableBand = interpolate(dist, vel, band, d, radial); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java index 4f045cd..7eab080 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java @@ -17,6 +17,8 @@ private ShotTable() {} /** Ball radius plus edge margin: how far inside the opening's side edges the ball centre must pass. */ public static final double LATERAL_CLEARANCE_M = 0.056; public static final double MIN_BAND_RPM = 100; + /** The table's distances are to the near lip; the tag cluster origin is the opening centre, this far beyond it. */ + public static final double LIP_TO_CENTRE_M = 0.154; public static final double[] DIST_M = {0.6, 0.7, 0.8, 0.9, 1, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, 2.9, 3}; public static final double[] VEL_MPS = {-1, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1}; diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java index 1deb91a..dbc6fe4 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java @@ -27,7 +27,7 @@ public class ShotSolverTest { 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); + return new ShotSolver(DIST, VEL, RPM, BAND, TOF, 60, 0.002, shooterOffsetM, 0, feedDelayS, 200, 0.508, 0.0555, 3500); } @Test @@ -111,7 +111,7 @@ public void outOfRangeIsInvalidAndIdlesAtNearestEdge() { @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); + ShotSolver s = new ShotSolver(DIST, VEL, noStationary, BAND, TOF, 60, 0.002, 0, 0, 0, 200, 0.508, 0.0555, 3500); assertEquals(3500, s.solve(0, 0, 0, 0, 0, 2, 0).idleRpm, EPS); } @@ -130,7 +130,7 @@ public void missingTimeOfFlightIsInvalid() { {0.8, 0.8, 0.8}, {Double.NaN, Double.NaN, Double.NaN}, {1.2, 1.2, 1.2}}; - ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, tofMissing, 60, 0.002, 0, 0, 200, 0.508, 0.0555, 3500); + ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, tofMissing, 60, 0.002, 0, 0, 0, 200, 0.508, 0.0555, 3500); ShotSolver.Shot shot = s.solve(0, 0, 0, 0, 0, 2, 0); assertFalse(shot.valid); assertEquals("NO SHOT", shot.reason); @@ -139,7 +139,7 @@ public void missingTimeOfFlightIsInvalid() { @Test public void zeroHorizontalExitSpeedIsInvalid() { // 90 deg launch: horizontal exit speed is ~0, so no tangential lead is possible. - ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, TOF, 90, 0.002, 0, 0, 200, 0.508, 0.0555, 3500); + ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, TOF, 90, 0.002, 0, 0, 0, 200, 0.508, 0.0555, 3500); ShotSolver.Shot shot = s.solve(0, 0, 0, 0, 0.5, 2, 0); assertFalse(shot.valid); assertEquals("NO SHOT", shot.reason); @@ -166,6 +166,15 @@ public void headingToleranceNarrowsWithDistance() { assertTrue(far.headingToleranceRad < near.headingToleranceRad); } + @Test + public void lipToCentreShortensTheTableDistance() { + // Table distance is to the near lip; the tracked target is the opening centre, 0.15 m further out. + ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, TOF, 60, 0.002, 0, 0.15, 0, 200, 0.508, 0.0555, 3500); + ShotSolver.Shot shot = s.solve(0, 0, 0, 0, 0, 2, 0); + assertEquals(1.85, shot.distanceM, EPS); + assertEquals(0, shot.headingRad, EPS); + } + @Test public void headingErrorWrapsAndIsPositiveWhenTargetIsLeft() { ShotSolver.Shot s = solver(0, 0).solve(0, 0, 0, 0, 0, 0, 2); // target heading +90 deg diff --git a/tools/shots/README.md b/tools/shots/README.md index e385136..9a87f86 100644 --- a/tools/shots/README.md +++ b/tools/shots/README.md @@ -45,6 +45,11 @@ Record here, then copy into `inputs.json` and regenerate. Time of flight at two distances, from the same video, checked against the table's `TOF_S` column. +The table's distances are to the Cell's near lip, but the tag cluster's +reported position (what `TargetTracker` places) is the opening centre; +`ShotTable.LIP_TO_CENTRE_M`, generated alongside the table, converts one to +the other before the lookup. + ## Why the tolerance is 50 RPM With a fixed launch angle the near lip sets the lowest speed that scores and diff --git a/tools/shots/shots.js b/tools/shots/shots.js index c2b8188..4c91173 100644 --- a/tools/shots/shots.js +++ b/tools/shots/shots.js @@ -137,7 +137,9 @@ ' 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 MIN_BAND_RPM = ' + num(2 * p.rpmToleranceRpm) + ';\n' + + ' /** The table\'s distances are to the near lip; the tag cluster origin is the opening centre, this far beyond it. */\n' + + ' public static final double LIP_TO_CENTRE_M = ' + num(p.openingLengthM * Math.cos(p.openingTiltDeg * Math.PI / 180) / 2) + ';\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' + From 238de3bc84db98790b8b68ca89b965129e3d1d7b Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:25:38 +0800 Subject: [PATCH 31/37] feature: shooter follows the table whenever the cell is tracked, Y only steers --- README.md | 2 +- .../org/firstinspires/ftc/teamcode/Robot.java | 34 +++++++++++-------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index 0cffc30..d881264 100644 --- a/README.md +++ b/README.md @@ -150,7 +150,7 @@ or right (blue). | Left trigger (hold) | Intake and indexer reverse | | Right bumper | Shooter on / off | | X (hold) | Fire: feeds only when at speed, and when aiming also only with a valid shot and heading inside the Cell width | -| 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 | +| Y (hold) | Aim: rotation tracks the solved shot heading. The shooter target follows the table whenever the Cell position is known, aiming or not. | | D-pad up / down | Trim every shooter target by ±50 RPM for the rest of the run | ## Testing on the robot diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index e879c0d..24a2a9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -109,14 +109,18 @@ public void periodic() { 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. + // Solve whenever the Cell is tracked, Y or not, so the wheels pre-spin at the right speed. + ShotSolver.Shot shot = tracker.hasTarget(now) + ? solver.solve(x, y, heading, vel[0], vel[1], tracker.getX(), tracker.getY()) + : null; + double headingErr = shot != null ? shot.headingErrorRad(heading) : 0; + shooter.setTargetRpm(shot != null ? (shot.valid ? shot.rpm : shot.idleRpm) : Constants.SHOOTER_SETPOINT_RPM); + + // Y governs rotation only: solver heading if we have a shot, 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))); + if (aiming && shot != null) { + rotate = aimRotation(Math.toDegrees(headingErr)); } else if (aiming && vision.hasTarget()) { rotate = aimRotation(vision.getBearingDeg()); } @@ -124,11 +128,11 @@ public void periodic() { 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. + // Fire gate. Aiming 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; + boolean ready = (aiming && shot != null) + ? shot.valid && shooter.atSpeed() && Math.abs(headingErr) < shot.headingToleranceRad + : shooter.atSpeed(); double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER); double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); boolean firing = false; @@ -147,10 +151,10 @@ public void periodic() { indexer.stop(); } - if (shot != null && log != null) { + if (aiming && 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); + headingErr, shot.valid ? 1 : 0, firing ? 1 : 0); } telemetry.addData("Alliance", vision.getAlliance()); @@ -161,11 +165,11 @@ public void periodic() { 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 + telemetry.addData("Shot", shot == null ? "hold Y" : (aiming ? "AIM " : "") + (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)); + Math.toDegrees(headingErr), 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(); From e2412c15861e1963d2e0b8a992e86de7f27e941d Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:37:29 +0800 Subject: [PATCH 32/37] feature: shot table self-check tests the operator's inputs --- tools/shots/export.js | 2 +- tools/shots/index.html | 16 ++++++++++++---- tools/shots/shots.js | 12 +++++++----- 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/tools/shots/export.js b/tools/shots/export.js index 404f1cf..dbc5554 100644 --- a/tools/shots/export.js +++ b/tools/shots/export.js @@ -8,7 +8,7 @@ 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(); +const fails = Shots.selfCheck(p); if (fails.length) { console.error('Self-check failed, not exporting:\n ' + fails.join('\n ')); process.exit(1); diff --git a/tools/shots/index.html b/tools/shots/index.html index 3827402..6be77b2 100644 --- a/tools/shots/index.html +++ b/tools/shots/index.html @@ -87,6 +87,15 @@

Actions

let grid = null, params = null; +// Shows/hides the banner and reports pass/fail; also gates the export button. +let selfCheckFailed = false; +function showSelfCheck(fails) { + selfCheckFailed = fails.length > 0; + $('banner').style.display = selfCheckFailed ? 'block' : 'none'; + $('banner').textContent = selfCheckFailed ? 'Self-check failed, export disabled:\n' + fails.join('\n') : ''; + return selfCheckFailed; +} + $('compute').onclick = () => { params = readInputs(); $('status').textContent = 'computing...'; @@ -95,6 +104,7 @@

Actions

grid = Shots.solveGrid(params); $('distIdx').max = grid.dist.length - 1; $('velIdx').max = grid.vel.length - 1; $('velIdx').value = Math.round((grid.vel.length - 1) / 2); + showSelfCheck(Shots.selfCheck(params)); $('exportJava').disabled = selfCheckFailed; $('status').textContent = 'computed ' + grid.dist.length + ' x ' + grid.vel.length + ' cells in ' + ((performance.now() - t0) / 1000).toFixed(1) + ' s'; draw(); @@ -176,10 +186,8 @@

Actions

: 'RPM ' + rpm + ' band ' + band + ' RPM (' + grid.lo[i][j] + '-' + grid.hi[i][j] + ') TOF ' + tof.toFixed(2) + ' s heading tol +/-' + headingTolDeg.toFixed(1) + ' deg horizontal exit ' + uh.toFixed(2) + ' m/s'; } -// Self-check on load. A failure shows the banner and keeps export disabled. -const fails = Shots.selfCheck(); -const selfCheckFailed = fails.length > 0; -if (selfCheckFailed) { $('banner').style.display = 'block'; $('banner').textContent = 'Self-check failed, export disabled:\n' + fails.join('\n'); } +// Self-check on load, against the defaults. A failure shows the banner and keeps export disabled. +showSelfCheck(Shots.selfCheck()); diff --git a/tools/shots/shots.js b/tools/shots/shots.js index 4c91173..f61e6f0 100644 --- a/tools/shots/shots.js +++ b/tools/shots/shots.js @@ -151,11 +151,13 @@ '}\n'; } - // Two checks that fail loudly if the physics is broken. Export is refused when this returns anything. - function selfCheck() { + // Two checks that fail loudly if the physics is broken, or if the operator's inputs leave no usable + // shot. Export is refused when this returns anything. + function selfCheck(p0) { + const base = Object.assign({}, DEFAULTS, p0); 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 p = Object.assign({}, base, { 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]) { @@ -164,8 +166,8 @@ 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 })); + // 2. Standing still, at the operator's inputs, a longer shot needs more RPM and at least one exists. + const g = solveGrid(Object.assign({}, base, { velMinMps: 0, velMaxMps: 0 })); let last = -Infinity, any = false; for (let i = 0; i < g.dist.length; i++) { const r = g.rpm[i][0]; From 8a956552bf2885eed2069ae96b1c3a0944ebf2e4 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:39:51 +0800 Subject: [PATCH 33/37] feature: tighter aim gate, camera-constants warning, shot telemetry shows band and flight time --- .../java/org/firstinspires/ftc/teamcode/Robot.java | 12 ++++++++---- .../org/firstinspires/ftc/teamcode/ShotSolver.java | 2 +- .../ftc/teamcode/subsystems/OdometrySubsystem.java | 2 +- .../firstinspires/ftc/teamcode/ShotSolverTest.java | 9 +++++++++ 4 files changed, 19 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 24a2a9e..f6c01a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -74,6 +74,10 @@ public void initLoop() { if (driver.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) vision.setAlliance(Alliance.BLUE); telemetry.addData("Alliance (dpad L/R)", vision.getAlliance()); telemetry.addData("Camera", vision.isAvailable() ? "ok" : "NOT FOUND"); + if (Constants.CAMERA_FORWARD_M == 0 && Constants.CAMERA_LEFT_M == 0 + && Constants.CAMERA_YAW_RAD == 0 && Constants.CAMERA_PITCH_RAD == 0) { + telemetry.addData("Camera mount", "CONSTANTS NOT SET, shot distance will be wrong"); + } telemetry.addData("Camera sees", vision.getTargetName()); telemetry.update(); } @@ -130,8 +134,8 @@ public void periodic() { // Fire gate. Aiming with a solved shot: valid, at speed, and pointing inside the Cell width. boolean fire = driver.isDown(GamepadKeys.Button.X); - boolean ready = (aiming && shot != null) - ? shot.valid && shooter.atSpeed() && Math.abs(headingErr) < shot.headingToleranceRad + boolean ready = aiming + ? shot != null && shot.valid && shooter.atSpeed() && Math.abs(headingErr) < shot.headingToleranceRad : shooter.atSpeed(); double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER); double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); @@ -166,9 +170,9 @@ public void periodic() { ? 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" : (aiming ? "AIM " : "") + (shot.valid - ? String.format("OK d %.2f radial %+.2f tang %+.2f rpm %.0f err %+.1f deg tol %.1f", + ? String.format("OK d %.2f radial %+.2f tang %+.2f rpm %.0f err %+.1f deg tol %.1f band %.0f tof %.2f", shot.distanceM, shot.radialVel, shot.tangentialVel, shot.rpm, - Math.toDegrees(headingErr), Math.toDegrees(shot.headingToleranceRad)) + Math.toDegrees(headingErr), Math.toDegrees(shot.headingToleranceRad), shot.bandRpm, shot.timeOfFlightS) : 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()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java index a3f5256..665a1d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java @@ -110,7 +110,7 @@ public Shot solve(double x, double y, double headingRad, double vx, double vy, d 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); + double tolerance = Math.atan(halfOpeningM / (d + lipToCentreM)); // 5. Horizontal exit speed, needed both to gate the shot and (below) to compute the lead. // cos(90 deg) in doubles is ~6e-17, not exactly zero, so an unguarded ratio would clamp to a diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index f7914e9..4c3e0aa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -41,7 +41,7 @@ public Pose2D getPose() { return pinpoint.getPosition(); } - /** Field-frame velocity from the Pinpoint, metres per second, as {x, y}. */ + /** Velocity from the Pinpoint, metres per second, as {x, y}. Assumed field-frame; confirm with README rung 0 before any moving shot. */ public double[] getVelocity() { return new double[]{pinpoint.getVelX(DistanceUnit.METER), pinpoint.getVelY(DistanceUnit.METER)}; } diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java index dbc6fe4..e5f08c2 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java @@ -166,6 +166,15 @@ public void headingToleranceNarrowsWithDistance() { assertTrue(far.headingToleranceRad < near.headingToleranceRad); } + @Test + public void headingToleranceUsesCentreDistance() { + // Opening centre is 2 m away; the table distance to the near lip is 1.85 m (lipToCentre 0.15). + // The heading tolerance should use the 2 m centre distance, not the shortened table distance. + ShotSolver s = new ShotSolver(DIST, VEL, RPM, BAND, TOF, 60, 0.002, 0, 0.15, 0, 200, 0.508, 0.0555, 3500); + ShotSolver.Shot shot = s.solve(0, 0, 0, 0, 0, 2, 0); + assertEquals(Math.atan((0.254 - 0.0555) / 2.0), shot.headingToleranceRad, EPS); + } + @Test public void lipToCentreShortensTheTableDistance() { // Table distance is to the near lip; the tracked target is the opening centre, 0.15 m further out. From b2adf89196e0409f99cd5f8c3faecc9e67db5ef0 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 14:43:49 +0800 Subject: [PATCH 34/37] docs: spec and tools guide match what was built --- .../2026-09-20-shoot-on-the-move-design.md | 56 +++++++++++-------- tools/shots/README.md | 14 ++--- 2 files changed, 39 insertions(+), 31 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 4a8b0d2..93fac4a 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 @@ -1,6 +1,6 @@ # Shoot on the move, design -Lebob Robotics, FTC 29550 · BIOBUZZ 2026/27 · v1.0, 20 Sep 2026 +Lebob Robotics, FTC 29550 · BIOBUZZ 2026/27 · v1.1, 20 Sep 2026 ## Purpose @@ -109,12 +109,12 @@ 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. +at every distance the table keeps, 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: @@ -126,15 +126,16 @@ before any moving shot is attempted. 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. + with a selector for band width or time of flight 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. +A second button downloads the input form as `inputs.json`, which `export.js` +reads. 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 @@ -146,8 +147,10 @@ shows a red banner and disables export. 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`. +`rpm`, `headingRad`, `headingToleranceRad` (atan of the half opening, less +ball clearance, over the distance to the opening centre), `valid`, and for +telemetry `distance`, `radialVel`, `tangentialVel`, `timeOfFlight`, +`bandWidth`. Steps: @@ -170,7 +173,9 @@ Steps: ### `TargetTracker` (robot) -Holds the up-facing Cell's opening centre in field coordinates. On each cluster +Holds the up-facing Cell's opening centre in field coordinates. The table's +distances are to the near lip, so `ShotSolver` subtracts +`ShotTable.LIP_TO_CENTRE_M` before the lookup. 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 @@ -187,18 +192,21 @@ is in the CAD. - `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. + telemetry, for when the table is slightly off on the day. Changes under + 10 RPM are ignored and a retarget does not re-assert the run mode, to keep + hub writes down. - `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". +- `Robot`: the shooter follows the table whenever the Cell position is known, + aiming or not, and holds the stationary RPM for that distance when no shot + exists; with no known Cell it returns to the fixed setpoint. Holding Y + steers the robot to the solver's heading (the left stick still translates), + gates Fire (X) on a valid shot, the shooter at speed, heading error inside + the tolerance set by the Cell width, and a target that has not expired, and + turns the log on. Pressing A to zero the heading also clears the tracked + target, because the field frame has moved. The Driver Station shows one of + `CLOSING TOO FAST`, `BACKING TOO FAST`, `OUT OF RANGE` or `NO SHOT`. - `Constants`: feed delay, shooter offset, launch angle, exit speed constant, heading gains, target expiry, camera transform. @@ -212,7 +220,7 @@ dashboards are banned at events. ## Measurements needed before the first table -Done once on the robot, recorded in `tools/shots/measurements.md`: +Done once on the robot, recorded in `tools/shots/README.md`: 1. Launch angle, from CAD and checked with a protractor. 2. Exit height above the tiles. @@ -226,7 +234,7 @@ Done once on the robot, recorded in `tools/shots/measurements.md`: ## Testing -**JVM unit tests** (`ShotSolverTest`, `ShotTableTest`): +**JVM unit tests** (`ShotSolverTest`, `TargetTrackerTest`): - Interpolation returns grid values at grid points and the midpoint between neighbours. diff --git a/tools/shots/README.md b/tools/shots/README.md index 9a87f86..963378e 100644 --- a/tools/shots/README.md +++ b/tools/shots/README.md @@ -55,17 +55,17 @@ the other before the lookup. 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. +the table keeps 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: +held and the Cell position is known. Pull them with: adb pull /sdcard/FIRST/shots ./shots From dcc548cddf2c6d6e1077014ca68276868a86037e Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 15:01:19 +0800 Subject: [PATCH 35/37] feature: shot tool defaults for an 80 degree launch and 60 degree cell tilt, table regenerated --- .../firstinspires/ftc/teamcode/ShotTable.java | 142 ++++++++---------- .../2026-09-20-shoot-on-the-move-design.md | 13 +- tools/shots/README.md | 4 +- tools/shots/inputs.json | 4 +- tools/shots/shots.js | 8 +- 5 files changed, 77 insertions(+), 94 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java index 7eab080..ca7fe9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java @@ -3,13 +3,13 @@ /** * GENERATED by tools/shots (node tools/shots/export.js). Do not edit by hand: change tools/shots/inputs.json * and regenerate. Rows are distance to the near lip, columns are radial robot velocity (positive closing). - * Inputs: {"ball":"POLLEN","ballDiameterM":0.0711,"ballMassKg":0.0249,"dragCd":0.45,"launchAngleDeg":60,"exitHeightM":0.4,"exitSpeedPerRpm":0.0023,"shooterOffsetM":0.15,"rpmToleranceRpm":50,"lipHeightM":1.359,"openingWidthM":0.508,"openingLengthM":0.356,"openingTiltDeg":30,"marginM":0.02,"distMinM":0.6,"distMaxM":3,"distStepM":0.1,"velMinMps":-1,"velMaxMps":1,"velStepMps":0.1,"rpmMin":1500,"rpmMax":5500,"rpmStep":25,"dtS":0.002} + * Inputs: {"ball":"POLLEN","ballDiameterM":0.0711,"ballMassKg":0.0249,"dragCd":0.45,"launchAngleDeg":80,"exitHeightM":0.4,"exitSpeedPerRpm":0.0023,"shooterOffsetM":0.15,"rpmToleranceRpm":50,"lipHeightM":1.359,"openingWidthM":0.508,"openingLengthM":0.356,"openingTiltDeg":60,"marginM":0.02,"distMinM":0.1,"distMaxM":1.5,"distStepM":0.075,"velMinMps":-2.3,"velMaxMps":2.3,"velStepMps":0.1,"rpmMin":1500,"rpmMax":5500,"rpmStep":25,"dtS":0.002} */ public final class ShotTable { private ShotTable() {} public static final String BALL = "POLLEN"; - public static final double LAUNCH_ANGLE_DEG = 60; + public static final double LAUNCH_ANGLE_DEG = 80; public static final double EXIT_HEIGHT_M = 0.4; public static final double EXIT_SPEED_PER_RPM = 0.0023; public static final double SHOOTER_OFFSET_M = 0.15; @@ -18,92 +18,74 @@ private ShotTable() {} public static final double LATERAL_CLEARANCE_M = 0.056; public static final double MIN_BAND_RPM = 100; /** The table's distances are to the near lip; the tag cluster origin is the opening centre, this far beyond it. */ - public static final double LIP_TO_CENTRE_M = 0.154; + public static final double LIP_TO_CENTRE_M = 0.089; - public static final double[] DIST_M = {0.6, 0.7, 0.8, 0.9, 1, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, 2.9, 3}; - public static final double[] VEL_MPS = {-1, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1}; + public static final double[] DIST_M = {0.1, 0.175, 0.25, 0.325, 0.4, 0.475, 0.55, 0.625, 0.7, 0.775, 0.85, 0.925, 1, 1.075, 1.15, 1.225, 1.3, 1.375, 1.45}; + public static final double[] VEL_MPS = {-2.3, -2.2, -2.1, -2, -1.9, -1.8, -1.7, -1.6, -1.5, -1.4, -1.3, -1.2, -1.1, -1, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2, 2.1, 2.2, 2.3}; /** Band-centre flywheel RPM, [dist][vel]. NaN where no shot lands. */ public static final double[][] RPM = { - {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2400, 2375, 2350, 2350, 2350, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2437.5, 2400, 2400, 2375, 2362.5, 2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2475, 2450, 2425, 2400, 2387.5, 2375, 2362.5, 2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2550, 2500, 2475, 2450, 2425, 2400, 2387.5, 2375, 2362.5, 2350, 2337.5, 2350, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2600, 2550, 2525, 2500, 2475, 2450, 2425, 2400, 2387.5, 2375, 2362.5, 2362.5, 2350, 2337.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2650, 2612.5, 2575, 2550, 2512.5, 2487.5, 2462.5, 2437.5, 2425, 2400, 2387.5, 2375, 2362.5, 2362.5, 2362.5, 2350, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {2700, 2662.5, 2625, 2600, 2562.5, 2537.5, 2500, 2475, 2462.5, 2437.5, 2425, 2400, 2387.5, 2375, 2362.5, 2362.5, 2362.5, 2362.5, 2362.5, Double.NaN, Double.NaN}, - {2750, 2712.5, 2675, 2650, 2612.5, 2575, 2550, 2525, 2500, 2475, 2450, 2437.5, 2412.5, 2400, 2387.5, 2387.5, 2362.5, 2362.5, 2362.5, 2362.5, 2362.5}, - {2812.5, 2762.5, 2737.5, 2687.5, 2662.5, 2625, 2600, 2562.5, 2537.5, 2512.5, 2487.5, 2475, 2450, 2437.5, 2412.5, 2412.5, 2387.5, 2387.5, 2375, 2362.5, 2362.5}, - {2862.5, 2812.5, 2787.5, 2737.5, 2712.5, 2675, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2512.5, 2487.5, 2462.5, 2450, 2437.5, 2412.5, 2412.5, 2387.5, 2387.5, 2387.5}, - {2912.5, 2875, 2837.5, 2787.5, 2762.5, 2725, 2687.5, 2662.5, 2637.5, 2600, 2575, 2550, 2525, 2512.5, 2487.5, 2462.5, 2450, 2437.5, 2412.5, 2412.5, 2387.5}, - {2962.5, 2925, 2887.5, 2837.5, 2812.5, 2762.5, 2737.5, 2712.5, 2675, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2512.5, 2500, 2487.5, 2462.5, 2437.5, 2437.5, 2412.5}, - {3012.5, 2987.5, 2937.5, 2887.5, 2862.5, 2812.5, 2787.5, 2762.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2512.5, 2487.5, 2487.5, 2462.5, 2437.5}, - {3062.5, 3037.5, 2987.5, 2937.5, 2912.5, 2862.5, 2837.5, 2787.5, 2762.5, 2737.5, 2712.5, 2675, 2637.5, 2612.5, 2587.5, 2562.5, 2550, 2537.5, 2512.5, 2487.5, 2475}, - {3125, 3087.5, 3037.5, 2987.5, 2962.5, 2912.5, 2887.5, 2837.5, 2812.5, 2787.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5, 2587.5, 2562.5, 2537.5, 2525, 2512.5}, - {3175, 3137.5, 3087.5, 3037.5, 3012.5, 2962.5, 2937.5, 2887.5, 2862.5, 2812.5, 2787.5, 2762.5, 2737.5, 2712.5, 2687.5, 2637.5, 2625, 2612.5, 2587.5, 2562.5, 2537.5}, - {3225, 3187.5, 3137.5, 3087.5, 3062.5, 3012.5, 2975, 2937.5, 2912.5, 2862.5, 2837.5, 2812.5, 2762.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5, 2587.5, 2562.5}, - {3275, 3237.5, 3187.5, 3137.5, 3100, 3062.5, 3025, 2987.5, 2950, 2912.5, 2887.5, 2837.5, 2812.5, 2787.5, 2762.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5, 2612.5}, - {3325, 3275, 3237.5, 3187.5, 3150, 3112.5, 3062.5, 3037.5, 2987.5, 2962.5, 2925, 2887.5, 2862.5, 2837.5, 2800, 2762.5, 2737.5, 2712.5, 2687.5, 2662.5, 2637.5}, - {3375, 3325, 3287.5, 3237.5, 3200, 3162.5, 3112.5, 3075, 3037.5, 3012.5, 2962.5, 2937.5, 2900, 2862.5, 2837.5, 2812.5, 2787.5, 2762.5, 2737.5, 2712.5, 2687.5}, - {3425, 3375, 3337.5, 3287.5, 3250, 3200, 3162.5, 3125, 3087.5, 3050, 3012.5, 2987.5, 2950, 2912.5, 2887.5, 2850, 2825, 2787.5, 2762.5, 2737.5, 2712.5}, - {3475, 3425, 3375, 3337.5, 3300, 3250, 3212.5, 3175, 3137.5, 3100, 3062.5, 3025, 2987.5, 2962.5, 2925, 2887.5, 2862.5, 2837.5, 2812.5, 2787.5, 2762.5}, - {3525, 3475, 3425, 3387.5, 3337.5, 3300, 3262.5, 3212.5, 3175, 3137.5, 3100, 3062.5, 3037.5, 3000, 2962.5, 2937.5, 2912.5, 2875, 2850, 2812.5, 2787.5} + {Double.NaN, Double.NaN, Double.NaN, 5300, 5062.5, 4812.5, 4575, 4337.5, 4100, 3875, 3637.5, 3412.5, 3187.5, 2975, 2762.5, 2562.5, 2375, 2225, 2175, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, 5412.5, 5175, 4937.5, 4687.5, 4462.5, 4225, 4000, 3775, 3550, 3337.5, 3112.5, 2912.5, 2725, 2550, 2387.5, 2262.5, 2162.5, 2150, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5287.5, 5050, 4812.5, 4587.5, 4350, 4125, 3900, 3675, 3462.5, 3262.5, 3062.5, 2875, 2700, 2525, 2400, 2275, 2187.5, 2137.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5400, 5162.5, 4925, 4687.5, 4462.5, 4237.5, 4025, 3800, 3600, 3400, 3200, 3012.5, 2837.5, 2675, 2525, 2400, 2287.5, 2212.5, 2150, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5275, 5037.5, 4812.5, 4587.5, 4362.5, 4137.5, 3925, 3725, 3525, 3325, 3137.5, 2962.5, 2800, 2650, 2512.5, 2400, 2300, 2212.5, 2162.5, 2125, 2137.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5375, 5150, 4925, 4687.5, 4475, 4262.5, 4037.5, 3837.5, 3637.5, 3450, 3250, 3087.5, 2925, 2762.5, 2637.5, 2512.5, 2400, 2312.5, 2237.5, 2175, 2150, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5450, 5250, 5025, 4812.5, 4587.5, 4362.5, 4162.5, 3962.5, 3762.5, 3562.5, 3375, 3200, 3050, 2887.5, 2750, 2612.5, 2500, 2400, 2312.5, 2250, 2200, 2150, 2125, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5362.5, 5137.5, 4912.5, 4700, 4487.5, 4262.5, 4062.5, 3862.5, 3675, 3500, 3325, 3150, 3000, 2850, 2725, 2600, 2500, 2400, 2325, 2250, 2200, 2175, 2150, 2125, 2112.5, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5450, 5250, 5025, 4800, 4587.5, 4387.5, 4187.5, 3987.5, 3787.5, 3612.5, 3437.5, 3262.5, 3100, 2962.5, 2825, 2700, 2600, 2500, 2400, 2325, 2275, 2225, 2175, 2150, 2125, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5350, 5125, 4912.5, 4700, 4487.5, 4287.5, 4087.5, 3887.5, 3712.5, 3537.5, 3375, 3212.5, 3062.5, 2925, 2800, 2687.5, 2575, 2487.5, 2400, 2337.5, 2275, 2225, 2200, 2162.5, 2137.5, 2125, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5437.5, 5225, 5012.5, 4800, 4587.5, 4387.5, 4187.5, 4012.5, 3812.5, 3637.5, 3475, 3312.5, 3162.5, 3025, 2900, 2775, 2675, 2575, 2487.5, 2412.5, 2350, 2287.5, 2237.5, 2200, 2175, 2150, 2137.5, 2125, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5325, 5125, 4900, 4700, 4500, 4300, 4112.5, 3925, 3737.5, 3587.5, 3412.5, 3262.5, 3125, 2987.5, 2875, 2762.5, 2662.5, 2562.5, 2487.5, 2412.5, 2350, 2300, 2250, 2212.5, 2175, 2162.5, 2137.5, 2125, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5425, 5225, 5000, 4800, 4600, 4400, 4212.5, 4025, 3850, 3687.5, 3512.5, 3362.5, 3212.5, 3087.5, 2962.5, 2850, 2737.5, 2637.5, 2562.5, 2487.5, 2412.5, 2350, 2300, 2262.5, 2225, 2187.5, 2162.5, 2150, 2137.5, 2125, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5325, 5100, 4900, 4700, 4500, 4312.5, 4125, 3950, 3775, 3612.5, 3462.5, 3312.5, 3187.5, 3062.5, 2937.5, 2825, 2725, 2637.5, 2562.5, 2487.5, 2412.5, 2362.5, 2312.5, 2262.5, 2237.5, 2200, 2175, 2162.5, 2137.5, 2137.5, 2125, 2125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5425, 5200, 5000, 4800, 4600, 4412.5, 4225, 4050, 3875, 3712.5, 3562.5, 3412.5, 3262.5, 3137.5, 3012.5, 2912.5, 2812.5, 2712.5, 2637.5, 2537.5, 2487.5, 2412.5, 2362.5, 2312.5, 2275, 2237.5, 2212.5, 2187.5, 2162.5, 2150, 2137.5, 2125, 2125, 2075, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5300, 5100, 4900, 4700, 4500, 4325, 4150, 3975, 3812.5, 3650, 3500, 3362.5, 3237.5, 3112.5, 2987.5, 2887.5, 2787.5, 2700, 2612.5, 2537.5, 2487.5, 2412.5, 2362.5, 2312.5, 2287.5, 2237.5, 2212.5, 2187.5, 2162.5, 2162.5, 2137.5, 2137.5, 2125, 2125, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5400, 5200, 5000, 4800, 4600, 4425, 4237.5, 4062.5, 3900, 3750, 3600, 3450, 3312.5, 3187.5, 3075, 2962.5, 2862.5, 2775, 2687.5, 2612.5, 2537.5, 2487.5, 2412.5, 2362.5, 2337.5, 2287.5, 2262.5, 2225, 2200, 2187.5, 2162.5, 2150, 2137.5, 2137.5, 2125, 2125, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5300, 5100, 4900, 4700, 4512.5, 4337.5, 4162.5, 4000, 3837.5, 3687.5, 3537.5, 3400, 3275, 3162.5, 3050, 2937.5, 2850, 2762.5, 2675, 2612.5, 2537.5, 2487.5, 2425, 2375, 2337.5, 2287.5, 2262.5, 2237.5, 2212.5, 2187.5, 2162.5, 2162.5, 2137.5, 2137.5, 2137.5, 2125}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 5400, 5187.5, 5000, 4800, 4600, 4425, 4250, 4087.5, 3925, 3775, 3625, 3487.5, 3362.5, 3237.5, 3125, 3025, 2925, 2825, 2750, 2662.5, 2600, 2537.5, 2475, 2425, 2387.5, 2337.5, 2300, 2262.5, 2237.5, 2212.5, 2187.5, 2187.5, 2162.5, 2162.5, 2137.5, 2137.5} }; /** Width of the valid RPM band, [dist][vel]. NaN where no shot lands. */ public static final double[][] BAND_RPM = { - {75, 0, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {150, 125, 75, 25, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {150, 150, 150, 150, 100, 25, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {175, 150, 150, 150, 125, 150, 125, 50, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {150, 150, 150, 150, 125, 150, 125, 150, 125, 75, 0, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {150, 150, 150, 150, 150, 150, 125, 150, 125, 150, 125, 100, 25, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {150, 150, 150, 150, 150, 150, 150, 150, 125, 150, 125, 125, 150, 125, 50, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {150, 125, 150, 150, 125, 125, 125, 125, 150, 150, 125, 150, 125, 125, 125, 100, 75, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {150, 125, 150, 150, 125, 125, 150, 150, 125, 125, 150, 150, 125, 150, 125, 125, 125, 125, 125, 25, Double.NaN}, - {150, 125, 150, 150, 125, 150, 150, 150, 150, 150, 150, 125, 125, 150, 125, 125, 125, 125, 125, 125, 125}, - {125, 125, 125, 125, 125, 150, 150, 125, 125, 125, 125, 150, 150, 125, 125, 125, 125, 125, 150, 125, 125}, - {125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125, 125}, - {125, 150, 125, 125, 125, 150, 125, 125, 125, 150, 150, 150, 150, 125, 125, 125, 150, 125, 125, 125, 125}, - {125, 150, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 125}, - {125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125}, - {125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125, 150, 125, 125, 125, 150}, - {100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125}, - {100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 150, 125, 125, 125, 125}, - {100, 125, 125, 125, 125, 125, 100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125}, - {100, 125, 125, 125, 100, 125, 100, 125, 100, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125}, - {100, 100, 125, 125, 100, 125, 125, 125, 125, 125, 100, 125, 125, 125, 100, 125, 125, 125, 125, 125, 125}, - {100, 100, 125, 125, 100, 125, 125, 100, 125, 125, 125, 125, 100, 125, 125, 125, 125, 125, 125, 125, 125}, - {100, 100, 125, 125, 100, 100, 125, 100, 125, 100, 125, 125, 100, 125, 125, 100, 100, 125, 125, 125, 125}, - {100, 100, 100, 125, 100, 100, 125, 100, 125, 100, 125, 100, 125, 125, 100, 125, 125, 125, 125, 125, 125}, - {100, 100, 100, 125, 125, 100, 125, 125, 100, 125, 100, 125, 125, 100, 125, 125, 125, 100, 100, 125, 125} + {Double.NaN, Double.NaN, 25, 150, 175, 175, 200, 175, 200, 200, 225, 225, 225, 250, 275, 275, 300, 300, 200, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, 175, 150, 175, 175, 175, 200, 200, 200, 200, 225, 225, 225, 250, 250, 275, 275, 275, 150, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, 50, 175, 150, 175, 175, 200, 200, 200, 200, 225, 225, 225, 250, 250, 250, 250, 250, 225, 225, 75, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, 150, 175, 150, 175, 175, 175, 200, 200, 200, 200, 200, 225, 225, 250, 250, 250, 225, 225, 200, 200, 0, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, 75, 150, 175, 175, 175, 175, 175, 200, 200, 200, 200, 225, 225, 200, 250, 225, 250, 250, 225, 225, 200, 125, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 150, 150, 150, 175, 150, 175, 175, 175, 175, 200, 200, 225, 200, 225, 225, 225, 200, 225, 225, 200, 200, 200, 50, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 100, 150, 150, 175, 175, 175, 175, 175, 175, 175, 200, 200, 200, 225, 200, 225, 200, 200, 225, 200, 200, 200, 200, 200, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 175, 175, 175, 150, 175, 175, 175, 175, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 175, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 100, 150, 150, 150, 175, 175, 175, 175, 175, 175, 175, 175, 200, 175, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 50, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 0, 150, 150, 175, 150, 175, 175, 175, 175, 175, 175, 200, 175, 175, 200, 200, 175, 200, 175, 200, 175, 200, 200, 200, 175, 175, 200, 200, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 125, 150, 175, 150, 175, 175, 175, 175, 175, 175, 200, 175, 175, 200, 200, 200, 200, 200, 175, 175, 200, 175, 175, 200, 200, 200, 175, 200, 200, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 25, 150, 150, 150, 150, 150, 150, 175, 150, 175, 175, 175, 175, 200, 175, 200, 175, 175, 175, 175, 175, 200, 200, 200, 175, 200, 175, 175, 200, 200, 25, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 150, 150, 150, 150, 150, 150, 175, 150, 150, 175, 175, 175, 175, 175, 175, 200, 175, 175, 175, 175, 175, 200, 200, 175, 200, 175, 175, 200, 175, 200, 200, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 50, 150, 150, 150, 150, 150, 175, 150, 150, 150, 175, 175, 175, 175, 175, 175, 200, 200, 175, 175, 175, 175, 175, 175, 175, 175, 200, 200, 175, 175, 175, 200, 200, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 150, 150, 150, 150, 150, 175, 150, 150, 150, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 200, 175, 175, 175, 175, 200, 175, 200, 200, 100, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 50, 150, 150, 150, 150, 150, 150, 150, 150, 175, 150, 150, 175, 175, 175, 175, 175, 175, 150, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 200, 200, 25, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 150, 150, 150, 150, 150, 150, 175, 175, 150, 150, 150, 150, 175, 175, 150, 175, 175, 150, 175, 175, 175, 175, 175, 175, 175, 175, 175, 200, 200, 175, 175, 200, 175, 175, 200, 200, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 75, 150, 150, 150, 150, 175, 175, 175, 150, 175, 175, 175, 150, 150, 175, 150, 175, 150, 175, 150, 175, 175, 175, 150, 150, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 175, 200}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 150, 175, 150, 150, 150, 150, 150, 175, 150, 150, 150, 175, 175, 175, 150, 150, 150, 150, 150, 175, 150, 175, 150, 150, 175, 175, 150, 175, 175, 175, 175, 175, 175, 175, 175, 175} }; /** Time of flight at the band centre, seconds, [dist][vel]. */ public static final double[][] TOF_S = { - {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.518, 0.492, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.572, 0.546, 0.518, 0.5, 0.48, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.612, 0.584, 0.568, 0.544, 0.522, 0.5, 0.476, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.648, 0.626, 0.604, 0.58, 0.562, 0.542, 0.522, 0.502, 0.48, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.69, 0.662, 0.642, 0.62, 0.6, 0.578, 0.56, 0.54, 0.522, 0.502, 0.482, 0.47, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.724, 0.698, 0.678, 0.658, 0.636, 0.616, 0.596, 0.574, 0.558, 0.54, 0.522, 0.506, 0.488, 0.468, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.754, 0.732, 0.71, 0.69, 0.668, 0.648, 0.628, 0.608, 0.592, 0.572, 0.556, 0.538, 0.52, 0.508, 0.494, 0.476, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, - {0.784, 0.762, 0.742, 0.722, 0.7, 0.682, 0.66, 0.64, 0.624, 0.606, 0.59, 0.57, 0.554, 0.538, 0.52, 0.508, 0.494, 0.482, 0.468, Double.NaN, Double.NaN}, - {0.812, 0.792, 0.77, 0.752, 0.73, 0.71, 0.692, 0.672, 0.654, 0.636, 0.618, 0.602, 0.584, 0.568, 0.552, 0.54, 0.52, 0.508, 0.496, 0.484, 0.472}, - {0.844, 0.82, 0.802, 0.778, 0.76, 0.74, 0.722, 0.7, 0.682, 0.664, 0.646, 0.632, 0.614, 0.598, 0.58, 0.568, 0.55, 0.538, 0.524, 0.508, 0.498}, - {0.87, 0.846, 0.828, 0.806, 0.788, 0.768, 0.746, 0.73, 0.712, 0.694, 0.678, 0.66, 0.642, 0.624, 0.61, 0.596, 0.578, 0.568, 0.55, 0.538, 0.528}, - {0.894, 0.874, 0.854, 0.832, 0.814, 0.794, 0.774, 0.758, 0.74, 0.72, 0.704, 0.686, 0.67, 0.656, 0.638, 0.622, 0.608, 0.594, 0.576, 0.566, 0.548}, - {0.92, 0.9, 0.88, 0.858, 0.84, 0.818, 0.802, 0.784, 0.764, 0.744, 0.728, 0.712, 0.694, 0.678, 0.662, 0.648, 0.634, 0.618, 0.602, 0.59, 0.574}, - {0.944, 0.926, 0.904, 0.882, 0.866, 0.844, 0.826, 0.81, 0.788, 0.772, 0.756, 0.738, 0.722, 0.706, 0.69, 0.674, 0.658, 0.642, 0.63, 0.614, 0.598}, - {0.966, 0.95, 0.928, 0.906, 0.89, 0.868, 0.852, 0.83, 0.814, 0.798, 0.782, 0.762, 0.744, 0.728, 0.712, 0.696, 0.682, 0.67, 0.654, 0.638, 0.624}, - {0.992, 0.974, 0.952, 0.93, 0.914, 0.892, 0.876, 0.854, 0.838, 0.822, 0.8, 0.784, 0.768, 0.754, 0.738, 0.722, 0.706, 0.69, 0.674, 0.662, 0.65}, - {1.014, 0.996, 0.974, 0.952, 0.938, 0.916, 0.9, 0.878, 0.862, 0.842, 0.826, 0.81, 0.794, 0.778, 0.762, 0.742, 0.73, 0.716, 0.702, 0.686, 0.67}, - {1.036, 1.018, 0.996, 0.976, 0.96, 0.938, 0.92, 0.902, 0.886, 0.864, 0.85, 0.834, 0.812, 0.798, 0.782, 0.766, 0.752, 0.736, 0.722, 0.706, 0.692}, - {1.058, 1.04, 1.018, 0.998, 0.98, 0.962, 0.942, 0.924, 0.906, 0.888, 0.872, 0.852, 0.836, 0.822, 0.806, 0.792, 0.776, 0.76, 0.746, 0.732, 0.716}, - {1.08, 1.058, 1.04, 1.02, 1.002, 0.984, 0.962, 0.946, 0.926, 0.91, 0.892, 0.874, 0.86, 0.844, 0.828, 0.81, 0.794, 0.78, 0.764, 0.75, 0.736}, - {1.1, 1.08, 1.062, 1.04, 1.022, 1.004, 0.984, 0.966, 0.948, 0.934, 0.912, 0.898, 0.88, 0.862, 0.848, 0.832, 0.818, 0.804, 0.788, 0.774, 0.76}, - {1.12, 1.1, 1.082, 1.062, 1.044, 1.024, 1.006, 0.988, 0.97, 0.952, 0.934, 0.92, 0.902, 0.884, 0.87, 0.852, 0.838, 0.82, 0.806, 0.792, 0.778}, - {1.142, 1.12, 1.1, 1.082, 1.064, 1.044, 1.026, 1.01, 0.992, 0.974, 0.956, 0.938, 0.922, 0.906, 0.89, 0.872, 0.858, 0.844, 0.828, 0.814, 0.8}, - {1.162, 1.14, 1.12, 1.102, 1.082, 1.066, 1.048, 1.028, 1.01, 0.992, 0.976, 0.958, 0.942, 0.926, 0.908, 0.894, 0.88, 0.862, 0.848, 0.832, 0.818} + {Double.NaN, Double.NaN, Double.NaN, 2.032, 1.95, 1.862, 1.776, 1.686, 1.592, 1.5, 1.402, 1.304, 1.202, 1.1, 0.992, 0.884, 0.77, 0.658, 0.574, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, 2.07, 1.99, 1.906, 1.818, 1.732, 1.642, 1.552, 1.46, 1.364, 1.27, 1.168, 1.07, 0.974, 0.876, 0.776, 0.682, 0.588, 0.52, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.028, 1.946, 1.862, 1.778, 1.69, 1.602, 1.512, 1.42, 1.328, 1.236, 1.144, 1.05, 0.958, 0.864, 0.78, 0.692, 0.614, 0.542, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.064, 1.984, 1.902, 1.818, 1.734, 1.648, 1.562, 1.472, 1.386, 1.298, 1.208, 1.118, 1.03, 0.944, 0.86, 0.78, 0.702, 0.634, 0.566, 0.508, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.022, 1.942, 1.862, 1.778, 1.694, 1.608, 1.522, 1.438, 1.352, 1.266, 1.18, 1.094, 1.012, 0.932, 0.854, 0.782, 0.712, 0.644, 0.586, 0.532, 0.494, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.058, 1.98, 1.902, 1.818, 1.738, 1.656, 1.568, 1.486, 1.402, 1.32, 1.234, 1.154, 1.076, 0.994, 0.924, 0.85, 0.782, 0.718, 0.66, 0.602, 0.558, 0.512, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.086, 2.016, 1.938, 1.86, 1.78, 1.696, 1.616, 1.536, 1.454, 1.37, 1.288, 1.208, 1.134, 1.056, 0.984, 0.912, 0.846, 0.782, 0.722, 0.67, 0.62, 0.57, 0.528, 0.494, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.052, 1.976, 1.898, 1.82, 1.742, 1.658, 1.578, 1.498, 1.418, 1.342, 1.264, 1.186, 1.112, 1.038, 0.972, 0.904, 0.844, 0.782, 0.728, 0.674, 0.628, 0.59, 0.55, 0.512, 0.478, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.084, 2.014, 1.936, 1.858, 1.78, 1.704, 1.626, 1.546, 1.466, 1.39, 1.314, 1.236, 1.162, 1.094, 1.024, 0.96, 0.9, 0.842, 0.782, 0.732, 0.686, 0.642, 0.6, 0.564, 0.528, 0.498, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.048, 1.972, 1.896, 1.82, 1.742, 1.666, 1.588, 1.508, 1.434, 1.36, 1.286, 1.214, 1.144, 1.076, 1.012, 0.95, 0.89, 0.836, 0.782, 0.736, 0.69, 0.65, 0.614, 0.576, 0.544, 0.514, 0.488, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.08, 2.006, 1.932, 1.856, 1.78, 1.704, 1.628, 1.556, 1.476, 1.404, 1.332, 1.26, 1.19, 1.124, 1.062, 0.998, 0.942, 0.888, 0.834, 0.786, 0.742, 0.698, 0.658, 0.622, 0.59, 0.558, 0.53, 0.502, 0.478, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.04, 1.97, 1.894, 1.82, 1.746, 1.67, 1.596, 1.522, 1.446, 1.378, 1.304, 1.238, 1.172, 1.108, 1.048, 0.99, 0.936, 0.882, 0.834, 0.786, 0.744, 0.704, 0.666, 0.632, 0.598, 0.57, 0.54, 0.514, 0.492, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.074, 2.004, 1.928, 1.856, 1.784, 1.708, 1.636, 1.562, 1.49, 1.422, 1.348, 1.282, 1.214, 1.154, 1.094, 1.036, 0.978, 0.926, 0.878, 0.832, 0.788, 0.746, 0.708, 0.674, 0.64, 0.608, 0.578, 0.554, 0.528, 0.504, 0.484, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.038, 1.964, 1.892, 1.82, 1.746, 1.674, 1.602, 1.532, 1.46, 1.392, 1.326, 1.26, 1.2, 1.14, 1.08, 1.024, 0.97, 0.922, 0.876, 0.832, 0.788, 0.75, 0.714, 0.678, 0.648, 0.618, 0.59, 0.566, 0.538, 0.518, 0.496, 0.476, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.072, 1.998, 1.928, 1.856, 1.784, 1.712, 1.64, 1.572, 1.5, 1.434, 1.368, 1.302, 1.238, 1.178, 1.12, 1.066, 1.014, 0.964, 0.918, 0.868, 0.83, 0.788, 0.752, 0.716, 0.684, 0.654, 0.626, 0.6, 0.574, 0.55, 0.528, 0.508, 0.488, 0.462, Double.NaN, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.032, 1.962, 1.892, 1.82, 1.748, 1.678, 1.61, 1.54, 1.474, 1.406, 1.342, 1.28, 1.222, 1.164, 1.106, 1.054, 1.004, 0.956, 0.91, 0.866, 0.83, 0.788, 0.754, 0.718, 0.69, 0.658, 0.632, 0.606, 0.58, 0.562, 0.538, 0.52, 0.5, 0.482, Double.NaN, Double.NaN, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.066, 1.996, 1.926, 1.856, 1.784, 1.716, 1.646, 1.578, 1.51, 1.448, 1.384, 1.32, 1.258, 1.202, 1.146, 1.092, 1.042, 0.996, 0.95, 0.906, 0.864, 0.828, 0.788, 0.754, 0.726, 0.694, 0.668, 0.64, 0.614, 0.592, 0.568, 0.548, 0.528, 0.512, 0.492, 0.476, Double.NaN}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.03, 1.96, 1.89, 1.82, 1.75, 1.684, 1.616, 1.55, 1.484, 1.422, 1.358, 1.298, 1.24, 1.186, 1.134, 1.08, 1.034, 0.988, 0.942, 0.904, 0.862, 0.828, 0.792, 0.758, 0.728, 0.698, 0.672, 0.646, 0.622, 0.598, 0.576, 0.558, 0.536, 0.52, 0.504, 0.486}, + {Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, 2.062, 1.992, 1.926, 1.856, 1.784, 1.718, 1.65, 1.586, 1.52, 1.458, 1.396, 1.336, 1.28, 1.224, 1.17, 1.12, 1.072, 1.022, 0.98, 0.936, 0.898, 0.862, 0.824, 0.792, 0.762, 0.73, 0.702, 0.676, 0.652, 0.628, 0.604, 0.586, 0.566, 0.548, 0.528, 0.512} }; } 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 93fac4a..093117f 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 @@ -85,12 +85,12 @@ characterisation session (below) as defaults: (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 + 14 in tall, opening tilt 60° 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 +- Sweep ranges: distance 0.1 to 1.5 m in 0.075 m steps, radial velocity −2.3 to + +2.3 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 @@ -105,11 +105,12 @@ bounce out of the Cell are not modelled. 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 +the only knob. No polynomial fit: the grid is 19 by 47 and the hub interpolates it directly. -Running the sweep with the placeholder inputs shows the band is 125 to 175 RPM -at every distance the table keeps, for any launch angle from 45° to 75°: the +Running the sweep with the placeholder inputs shows the band is 150 to 225 RPM +at every distance the table keeps (80° launch, 60° opening tilt; it was 125 to +175 RPM for launch angles from 45° to 75° at a 30° tilt): 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 diff --git a/tools/shots/README.md b/tools/shots/README.md index 963378e..9802bc8 100644 --- a/tools/shots/README.md +++ b/tools/shots/README.md @@ -54,8 +54,8 @@ the other before the lookup. 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 -the table keeps and for any launch angle from 45° to 75°. The table only +that window is about 0.3 m/s of ball speed, 150 to 225 RPM, at every distance +the table keeps. 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 diff --git a/tools/shots/inputs.json b/tools/shots/inputs.json index 0b3e91a..9c7f37b 100644 --- a/tools/shots/inputs.json +++ b/tools/shots/inputs.json @@ -3,7 +3,7 @@ "ballDiameterM": 0.0711, "ballMassKg": 0.0249, "dragCd": 0.45, - "launchAngleDeg": 60, + "launchAngleDeg": 80, "exitHeightM": 0.40, "exitSpeedPerRpm": 0.0023, "shooterOffsetM": 0.15, @@ -11,6 +11,6 @@ "lipHeightM": 1.359, "openingWidthM": 0.508, "openingLengthM": 0.356, - "openingTiltDeg": 30, + "openingTiltDeg": 60, "marginM": 0.02 } diff --git a/tools/shots/shots.js b/tools/shots/shots.js index f61e6f0..b2f5d18 100644 --- a/tools/shots/shots.js +++ b/tools/shots/shots.js @@ -18,7 +18,7 @@ ballDiameterM: BALLS.POLLEN.ballDiameterM, ballMassKg: BALLS.POLLEN.ballMassKg, dragCd: 0.45, // guess; calibrate against measured landing distance - launchAngleDeg: 60, // measure on the robot + launchAngleDeg: 80, // 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 @@ -26,10 +26,10 @@ 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 + openingTiltDeg: 60, // 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, + distMinM: 0.1, distMaxM: 1.5, distStepM: 0.075, + velMinMps: -2.3, velMaxMps: 2.3, velStepMps: 0.1, rpmMin: 1500, rpmMax: 5500, rpmStep: 25, dtS: 0.002, }; From 98f8be9a2b1b26ff2bdc41abe5f2c892580c11ba Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Sun, 20 Sep 2026 15:02:22 +0800 Subject: [PATCH 36/37] fix: shot graph drawn to scale so the launch angle and cell tilt are not skewed --- tools/shots/index.html | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tools/shots/index.html b/tools/shots/index.html index 6be77b2..c99d193 100644 --- a/tools/shots/index.html +++ b/tools/shots/index.html @@ -151,7 +151,10 @@

Actions

traces.push({ x: [o.x0, o.x0], y: [0, o.y0], mode: 'lines', name: 'Cell front', line: { color: '#000', width: 2, dash: 'dot' } }); Plotly.react('fan', traces, { title: 'Shots at ' + d.toFixed(2) + ' m, radial ' + vr.toFixed(2) + ' m/s' + (Number.isNaN(chosen) ? ' (NO SHOT)' : ''), - xaxis: { title: 'distance (m)', range: [0, Math.max(3.5, o.x1 + 0.5)] }, yaxis: { title: 'height (m)', range: [0, 3.2] }, + // Equal metres per pixel on both axes, or the opening's tilt and the launch angle are drawn skewed. + // constrain 'domain' keeps the ranges and shrinks the plot area instead of stretching an axis. + xaxis: { title: 'distance (m)', range: [-0.2, Math.max(params.distMaxM + params.openingLengthM, o.x1) + 0.4], constrain: 'domain' }, + yaxis: { title: 'height (m)', range: [0, 3.2], scaleanchor: 'x', scaleratio: 1, constrain: 'domain' }, margin: { t: 40, r: 10 }, showlegend: false, annotations: [{ x: 0, y: params.exitHeightM, ax: -vr * 60, ay: 0, xref: 'x', yref: 'y', axref: 'pixel', ayref: 'pixel', showarrow: true, arrowhead: 2, text: 'robot ' + vr.toFixed(1) + ' m/s' }], }, { responsive: true }); From ab918102b7b15233c5ae31b6ea6b4df4fb6eafe4 Mon Sep 17 00:00:00 2001 From: Hazzer890 Date: Fri, 25 Sep 2026 23:03:10 +0800 Subject: [PATCH 37/37] feature: hood travel sim, sweeps hood angle and RPM to find the narrowest window that covers the envelope tools/shots/hood.js sweeps hood angle by RPM at every distance and radial velocity, keeps the shots that survive the hood and flywheel tolerances, and searches for the narrowest hood travel that covers every cell. hood.html plots the valid region, chosen angle, side view, coverage against travel and a field map. shots.js splits simulate into fly and land so one flight serves every distance, ShotTable.java regenerates byte-identical. --- tools/shots/README.md | 44 +++++++++ tools/shots/hood.html | 202 ++++++++++++++++++++++++++++++++++++++++++ tools/shots/hood.js | 131 +++++++++++++++++++++++++++ tools/shots/shots.js | 55 ++++++++---- 4 files changed, 415 insertions(+), 17 deletions(-) create mode 100644 tools/shots/hood.html create mode 100644 tools/shots/hood.js diff --git a/tools/shots/README.md b/tools/shots/README.md index 9802bc8..af74242 100644 --- a/tools/shots/README.md +++ b/tools/shots/README.md @@ -17,6 +17,12 @@ These tools work that out on a laptop and give the robot a table. `inputs.json` and commit both files together. - `inputs.json`: the measured constants behind the committed table. +- `hood.js`: sizes an adjustable hood. Sweeps hood angle and RPM at every + distance and radial velocity in the envelope, keeps the shots that survive + the hood and flywheel tolerances, and searches for the narrowest hood travel + that still covers every cell. `node tools/shots/hood.js` prints it. +- `hood.html`: the same, with plots. Open it, press Compute, move the sliders. + ## What the three panels show 1. Trajectory fan. Every flywheel speed at the chosen distance and radial @@ -27,6 +33,44 @@ These tools work that out on a laptop and give the robot a table. driver cannot exceed. 3. Heatmap of the table. Grey cells have no shot. +## Sizing the hood + +`hood.html` answers one question: how far does the hood have to travel? A +shot is kept only when every angle within the hood tolerance and every speed +within the flywheel tolerance still scores, which is 4414's inscribed +tolerance region drawn as a rectangle. Each shot's margin is its distance from +the nearest miss, in tolerances, so larger is safer. The recommended window is +the narrowest one where every cell that can be covered at all has such a shot, +ties going to the higher mean margin. + +- Valid region. Hood angle against RPM at the slider's cell. Dark cannot + score, pale scores but not with the tolerances, colour is the margin. The + star is the chosen shot, the white box its tolerance rectangle, the dashed + lines the recommended window. +- Chosen angle against distance, one line per radial velocity. This is what + the runtime table would hold. +- Cells covered against hood travel. Where it meets the dotted line is the + recommended travel; the slope before it is what each extra degree buys. + +- Side view. The hood's recommended wedge at the exit point, the chosen + angle through it, the chosen arc in blue, and the four corners of its + tolerance rectangle faintly, green where they score. +- Field map. Top-down, coloured by margin, hood angle or RPM of the shot from + each robot position at the slider's radial velocity. The Cell's field + position and facing are placeholders on the form until read from the + manual. Grey means behind the Cell, outside the swept distances, or no + robust shot. The map assumes a head-on shot at every bearing; a robot 60 + degrees off the Cell's axis is coloured the same as one dead ahead, which + the sim does not model. + +Cells listed as uncoverable have no robust shot at any swept angle. With the +placeholder inputs those are the very close shots and fast approaches at short +range; extending the sweep to 89 degrees reaches some of them. + +Self-check: with the sweep pinned to the fixed launch angle and no hood error, +the coverable stationary cells must be exactly the usable cells of the RPM +table. The page shows a red banner if not; the node script refuses to run. + ## Measurements Record here, then copy into `inputs.json` and regenerate. diff --git a/tools/shots/hood.html b/tools/shots/hood.html new file mode 100644 index 0000000..a254d9a --- /dev/null +++ b/tools/shots/hood.html @@ -0,0 +1,202 @@ + + + + +BIOBUZZ hood travel + + + + + + +
+ +

Actions

+ + +

+
+
+ +
Press Compute. The sweep takes a few seconds.
+
+ + + + +
+
+
+
+
+
+
+ + + diff --git a/tools/shots/hood.js b/tools/shots/hood.js new file mode 100644 index 0000000..08fb388 --- /dev/null +++ b/tools/shots/hood.js @@ -0,0 +1,131 @@ +#!/usr/bin/env node +// Hood travel sizing: how far the hood must move so every shot in the envelope has a robust +// (angle, RPM) point. Reuses the ballistics in shots.js unchanged. Loaded by hood.html in a browser; +// `node tools/shots/hood.js` prints the recommendation. +(function (root, factory) { + if (typeof module === 'object' && module.exports) module.exports = factory(require('./shots.js')); + else root.Hood = factory(root.Shots); +})(typeof self !== 'undefined' ? self : this, function (Shots) { + 'use strict'; + + const DEFAULTS = Object.assign({}, Shots.DEFAULTS, { + hoodMinDeg: 30, hoodMaxDeg: 85, hoodStepDeg: 1, + hoodTolDeg: 1, // hood angle error the chosen shot must survive + velMinMps: -1, velMaxMps: 1, velStepMps: 0.2, + rpmStep: 50, // coarser than the RPM table: 56 angles x 81 speeds per cell + }); + + // Margin of a scoring (angle, RPM) point: distance, in tolerance units, to the nearest miss or + // grid edge. `robust` needs every point within one hood tolerance by one RPM tolerance to score + // (a rectangle, 4414's inscribed region), `margin` is the Euclidean distance so ties break toward + // the middle of the valid region. Capped at RADIUS tolerances. + const RADIUS = 3; + function grade(ok, ia, ir, p) { + const sa = p.hoodStepDeg / p.hoodTolDeg, sr = p.rpmStep / p.rpmToleranceRpm; // steps -> tolerances + const ka = Math.ceil(RADIUS / sa), kr = Math.ceil(RADIUS / sr); + let cheb = RADIUS, euc = RADIUS; + for (let da = -ka; da <= ka; da++) for (let dr = -kr; dr <= kr; dr++) { + const a = ia + da, r = ir + dr; + const miss = a < 0 || a >= ok.length || r < 0 || r >= ok[0].length || !ok[a][r]; + if (!miss) continue; + const ta = da ? Math.abs(da) * sa : 0, tr = dr ? Math.abs(dr) * sr : 0; + cheb = Math.min(cheb, Math.max(ta, tr)); euc = Math.min(euc, Math.hypot(ta, tr)); + } + return { robust: cheb > 1, margin: euc }; + } + + // Every flight in the sweep at one radial velocity, indexed [angle][rpm]. Distance-independent. + function flights(velMps, p) { + const angles = Shots.axis(p.hoodMinDeg, p.hoodMaxDeg, p.hoodStepDeg), rpms = Shots.axis(p.rpmMin, p.rpmMax, p.rpmStep); + const maxX = Shots.opening(p.distMaxM, p).x1 + 0.01; // nothing to hit beyond the farthest Cell's far edge + return { angles, rpms, pts: angles.map(a => rpms.map(r => Shots.fly(r, velMps, Object.assign({}, p, { launchAngleDeg: a }), maxX))) }; + } + + // One cell of the envelope: which (angle, RPM) points score, their margins, and the best RPM at + // each angle so the window search only has to look at angles. + function cell(distM, velMps, f, p) { + const ok = f.pts.map(row => row.map(pts => Shots.land(pts, distM, p).result === 'score')); + const margin = ok.map((row, ia) => row.map((v, ir) => { if (!v) return 0; const g = grade(ok, ia, ir, p); return g.robust ? g.margin : 0; })); + const byAngle = margin.map(row => { let best = { ir: -1, s: 0 }; row.forEach((s, ir) => { if (s > best.s) best = { ir, s }; }); return best; }); + return { distM, velMps, angles: f.angles, rpms: f.rpms, ok, margin, byAngle }; + } + + function sweep(p, onProgress) { + const dist = Shots.axis(p.distMinM, p.distMaxM, p.distStepM), vel = Shots.axis(p.velMinMps, p.velMaxMps, p.velStepMps); + const cells = []; + dist.forEach(d => vel.forEach(v => cells.push(null))); + vel.forEach((v, j) => { + const f = flights(v, p); + dist.forEach((d, i) => { cells[i * vel.length + j] = cell(d, v, f, p); if (onProgress) onProgress(i * vel.length + j + 1, cells.length); }); + }); + return { dist, vel, cells, angles: cells[0].angles, rpms: cells[0].rpms }; + } + + // Best shot for a cell using only angles in [lo, hi] (indices), or null. + function bestIn(c, lo, hi) { + let best = null; + for (let ia = lo; ia <= hi; ia++) { const b = c.byAngle[ia]; if (b.s > 0 && (!best || b.s > best.s)) best = { ia, ir: b.ir, s: b.s }; } + return best; + } + + // Every [lo, hi] window. The recommendation is the narrowest that covers every coverable cell, + // ties broken by mean margin. coverage[w] is the most cells any window of width w covers. + function windows(g) { + const n = g.angles.length; + const coverable = g.cells.filter(c => bestIn(c, 0, n - 1)).length; + const coverage = new Array(n).fill(0); + let rec = null; + for (let lo = 0; lo < n; lo++) for (let hi = lo; hi < n; hi++) { + let covered = 0, sum = 0; + for (const c of g.cells) { const b = bestIn(c, lo, hi); if (b) { covered++; sum += b.s; } } + coverage[hi - lo] = Math.max(coverage[hi - lo], covered); + if (covered === coverable) { + const w = hi - lo, mean = covered ? sum / covered : 0; + if (!rec || w < rec.hi - rec.lo || (w === rec.hi - rec.lo && mean > rec.mean)) rec = { lo, hi, mean }; + } + } + const uncoverable = g.cells.filter(c => !bestIn(c, 0, n - 1)).map(c => ({ distM: c.distM, velMps: c.velMps })); + return { rec, coverable, coverage, uncoverable }; + } + + // With the sweep pinned to the fixed launch angle and no hood error, the coverable stationary + // cells must be exactly the usable cells of the existing RPM table. + function selfCheck(p0) { + const p = Object.assign({}, DEFAULTS, p0, { hoodMinDeg: p0.launchAngleDeg, hoodMaxDeg: p0.launchAngleDeg, hoodTolDeg: 0, velMinMps: 0, velMaxMps: 0, rpmStep: Shots.DEFAULTS.rpmStep }); + const g = sweep(p), t = Shots.solveGrid(p), fails = []; + g.cells.forEach((c, i) => { + const hood = !!bestIn(c, 0, 0), table = !Number.isNaN(t.rpm[i][0]); + if (hood !== table) fails.push('at ' + c.distM + ' m the hood sweep says ' + (hood ? 'shot' : 'no shot') + ' but the RPM table says ' + (table ? 'shot' : 'no shot')); + }); + return fails; + } + + function report(g, w) { + const f = a => g.angles[a].toFixed(0); + let s = w.rec + ? 'Recommended hood travel: ' + f(w.rec.lo) + ' to ' + f(w.rec.hi) + ' deg (' + (g.angles[w.rec.hi] - g.angles[w.rec.lo]) + ' deg), mean margin ' + w.rec.mean.toFixed(2) + ' tolerances\n' + : 'No window covers anything\n'; + const byDist = {}; + for (const c of w.uncoverable) (byDist[c.distM] = byDist[c.distM] || []).push(c.velMps); + const unc = Object.keys(byDist).map(d => { const v = byDist[d]; return d + ' m @ ' + (v.length === 1 ? v[0] : v[0] + '..' + v[v.length - 1]) + ' m/s'; }); + s += 'Covers ' + w.coverable + '/' + g.cells.length + ' cells; uncoverable at any swept angle: ' + (unc.length ? unc.join(', ') : 'none') + '\n'; + if (w.rec) { + s += 'Stationary shots inside the window:\n'; + for (const c of g.cells) if (Math.abs(c.velMps) < 1e-9) { const b = bestIn(c, w.rec.lo, w.rec.hi); s += ' ' + c.distM.toFixed(3) + ' m: ' + (b ? f(b.ia) + ' deg @ ' + c.rpms[b.ir] + ' RPM, margin ' + b.s : 'none') + '\n'; } + } + return s; + } + + return { DEFAULTS, grade, flights, cell, sweep, bestIn, windows, selfCheck, report }; +}); + +if (typeof require !== 'undefined' && require.main === module) { + const fs = require('fs'), path = require('path'); + const Hood = module.exports; + const p = Object.assign({}, Hood.DEFAULTS, JSON.parse(fs.readFileSync(path.join(__dirname, 'inputs.json'), 'utf8'))); + const fails = Hood.selfCheck(p); + if (fails.length) { console.error('Self-check failed:\n ' + fails.join('\n ')); process.exit(1); } + const t0 = Date.now(); + const g = Hood.sweep(p); + process.stdout.write(Hood.report(g, Hood.windows(g)) + 'swept ' + g.cells.length + ' cells in ' + ((Date.now() - t0) / 1000).toFixed(1) + ' s\n'); +} diff --git a/tools/shots/shots.js b/tools/shots/shots.js index b2f5d18..2f71c43 100644 --- a/tools/shots/shots.js +++ b/tools/shots/shots.js @@ -59,31 +59,52 @@ 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) { + // Fly one ball from the shooter (x = 0, y = exit height) with nothing to hit, until it reaches the + // tiles, 5 s, or x past maxXM (when every target of interest is behind). 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. Each point is + // [x, y, vy]. The flight does not depend on the Cell's distance, so one flight serves every distance. + function fly(rpm, radialVelMps, p, maxXM) { + if (maxXM === undefined) maxXM = Infinity; 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 pts = [[x, y, vy]]; + while (t < 5 && y >= 0 && x <= maxXM) { 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; + x += (vx + vx1) / 2 * p.dtS; y += (vy + vy1) / 2 * p.dtS; vx = vx1; vy = vy1; t += p.dtS; + pts.push([x, y, vy]); + } + return pts; + } + + // Where a flight ends against a Cell whose near lip is at x = distM: 'score', or why it missed. + // Returns the point index where it ended. Nothing can happen before x reaches the lip, and x is + // monotonic, so the walk starts at the first segment that gets there. + function land(pts, distM, p) { + const r = p.ballDiameterM / 2; + const o = opening(distM, p); + const clear = (r + p.marginM) / p.openingLengthM; // edge clearance as a fraction of the opening + if (pts[pts.length - 1][0] < o.x0) return { result: 'short', end: pts.length - 1 }; + let lo = 1, hi = pts.length - 1; + while (lo < hi) { const mid = (lo + hi) >> 1; if (pts[mid][0] < o.x0) lo = mid + 1; else hi = mid; } + for (let i = lo; i < pts.length; i++) { + const [x, y] = pts[i - 1], [nx, ny, vy] = pts[i]; 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; } + if (u !== null && vy < 0) return { result: u < clear ? 'lip' : u > 1 - clear ? 'far' : 'score', end: i }; + if (nx >= o.x0 && ny < o.y0) return { result: 'short', end: i }; // into the front face below the lip + if (nx > o.x1 && ny >= o.y1) return { result: 'long', end: i }; // over the far edge + if (ny < 0) return { result: nx > o.x1 ? 'long' : 'short', end: i }; } - return { points: pts, result: result, tof: t }; + return { result: pts[pts.length - 1][0] > o.x1 ? 'long' : 'short', end: pts.length - 1 }; // flight cut off past the far edge + } + + // Fly one ball at a Cell whose near lip is at x = distM. Points stop where the flight ended. + function simulate(rpm, radialVelMps, distM, p) { + const pts = fly(rpm, radialVelMps, p), l = land(pts, distM, p); + return { points: pts.slice(0, l.end + 1), result: l.result, tof: l.end * p.dtS }; } // The contiguous band of RPMs that score, its centre and the time of flight at the centre. @@ -180,5 +201,5 @@ return fails; } - return { DEFAULTS: DEFAULTS, BALLS: BALLS, axis: axis, opening: opening, simulate: simulate, band: band, solveGrid: solveGrid, toJava: toJava, selfCheck: selfCheck }; + return { DEFAULTS: DEFAULTS, BALLS: BALLS, axis: axis, opening: opening, fly: fly, land: land, simulate: simulate, band: band, solveGrid: solveGrid, toJava: toJava, selfCheck: selfCheck }; });