Feature/shoot on the move

.github/workflows/build.yml

@@ -52,3 +52,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

README.md

@@ -114,3 +114,79 @@ 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: 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. 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
+
+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`.
+
+### 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.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java

@@ -0,0 +1,3 @@
+package org.firstinspires.ftc.teamcode;
+
+public enum Alliance { RED, BLUE }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java

@@ -0,0 +1,73 @@
+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 = -105.0;   // measured, from main 12cad4b
+
+    // 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 = 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;
+    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;
+
+    // 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";
+}

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();

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;
+    }
+}