diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index c17819d..adad6c2 100644 --- a/.github/workflows/build.yml +++ b/.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 diff --git a/README.md b/README.md index 6fd496f..d881264 100644 --- a/README.md +++ b/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. 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..8b34266 --- /dev/null +++ b/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"; +} 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/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/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 3b08345..f6c01a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -1,36 +1,64 @@ 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 com.qualcomm.robotcore.hardware.VoltageSensor; 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; 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; - private final Gamepad driver; + private final GamepadEx driver; + private final VoltageSensor battery; public final MecanumDriveSubsystem drive; public final OdometrySubsystem odometry; + public final IntakeSubsystem intake; + public final IndexerSubsystem indexer; + public final ShooterSubsystem shooter; + public final VisionSubsystem vision; - public Robot( - HardwareMap hardwareMap, - Telemetry telemetry, - Gamepad driver - ) { + 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.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( + 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 = 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); + } + battery = hardwareMap.voltageSensor.iterator().next(); CommandScheduler.getInstance().reset(); - // Construct subsystems. drive = new MecanumDriveSubsystem(hardwareMap); odometry = new OdometrySubsystem(hardwareMap); - // intake = new IntakeSubsystem(hardwareMap); - // outtake = new OuttakeSubsystem(hardwareMap); + intake = new IntakeSubsystem(hardwareMap); + indexer = new IndexerSubsystem(hardwareMap); + shooter = new ShooterSubsystem(hardwareMap); + vision = new VisionSubsystem(hardwareMap); } /** Called once when the OpMode enters INIT. */ @@ -38,25 +66,132 @@ 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", 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(); + } + + /** 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(); + long now = System.nanoTime(); - if (driver.a) { + 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); - // 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)); + // 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()); + } - telemetry.addData("Pose", odometry.getPose()); + // 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(); + if (aiming && shot != null) { + rotate = aimRotation(Math.toDegrees(headingErr)); + } else if (aiming && vision.hasTarget()) { + rotate = aimRotation(vision.getBearingDeg()); + } + + boolean fieldCentric = !driver.isDown(GamepadKeys.Button.LEFT_BUMPER); + drive.drive(driver.getLeftY(), driver.getLeftX(), rotate, fieldCentric, heading); + + // 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 + : shooter.atSpeed(); + 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 (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(), + headingErr, 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" : (aiming ? "AIM " : "") + (shot.valid + ? 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), 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()); telemetry.update(); } /** Called once when the OpMode stops. */ public void stop() { + if (log != null) log.close(); + 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/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/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; + } +} 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..665a1d4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotSolver.java @@ -0,0 +1,162 @@ +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; + /** 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; + 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 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 lipToCentreM, + 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.lipToCentreM = lipToCentreM; + 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 (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. + 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, 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); + 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 + 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 + // 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) || 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); + } + // 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); + } + + /** 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/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..ca7fe9a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShotTable.java @@ -0,0 +1,91 @@ +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":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 = 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; + 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; + /** 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.089; + + 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, 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 = { + {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, 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/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..6b25fde --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TargetTracker.java @@ -0,0 +1,117 @@ +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; + } + + /** 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/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); + } +} 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..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,7 +4,8 @@ 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.Constants; +import org.firstinspires.ftc.teamcode.MecanumKinematics; /** * Drivetrain subsystem for a four-motor mecanum base, with optional field-centric @@ -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); @@ -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/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index 26f8ff5..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 @@ -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,12 +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() { - pinpoint.setOffsets(0.0, -105.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, @@ -40,6 +41,11 @@ public Pose2D getPose() { return pinpoint.getPosition(); } + /** 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)}; + } + /** 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 new file mode 100644 index 0000000..201eaf6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -0,0 +1,118 @@ +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; + 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); + 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); + } + } + + /** Back into RUN_USING_ENCODER so the hub PID holds the setpoint. */ + public void spinUp() { + running = true; + 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. 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 (Math.abs(rpm - targetRpm) < Constants.SHOOTER_RETARGET_RPM) return; + targetRpm = rpm; + if (running) sendVelocity(); + } + + /** 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) sendVelocity(); + } + + 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; + * the controller keeps PIDF coefficients per mode, so the constructor's gains survive. + */ + public void idle() { + running = false; + for (DcMotorEx m : new DcMotorEx[]{left, right}) { + m.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + m.setPower(0); + } + } + + public void toggle() { + if (running) idle(); else spinUp(); + } + + 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 current target. */ + public boolean atSpeed() { + double target = getTargetRpm(); + return running + && Math.abs(leftRpm - target) < Constants.SHOOTER_TOLERANCE_RPM + && Math.abs(rightRpm - target) < Constants.SHOOTER_TOLERANCE_RPM; + } + + public double getLeftRpm() { + return leftRpm; + } + + public double getRightRpm() { + 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 new file mode 100644 index 0000000..815f0a8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/VisionSubsystem.java @@ -0,0 +1,113 @@ +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.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; +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". + * 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 AprilTagProcessor processor; + private VisionPortal portal; + private Alliance alliance = Alliance.RED; + private AprilTagClusterDetection target; + + 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)) + .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) { + 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() { + if (portal == null) return; + 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; + } + + /** 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; + } + + /** Turn off the Driver Station preview once the match starts to save CPU. */ + public void stopLiveView() { + if (portal != null) portal.stopLiveView(); + } + + public void close() { + if (portal != null) portal.close(); + } +} 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); + } +} 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); + } +} 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..e5f08c2 --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/ShotSolverTest.java @@ -0,0 +1,194 @@ +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, 0, 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, 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 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, 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, 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); + 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 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. + 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 + 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); + } +} 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..f2f4bb7 --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TargetTrackerTest.java @@ -0,0 +1,121 @@ +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)); + } + + @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 + } +} 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..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 @@ -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 @@ -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. @@ -82,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 @@ -102,16 +105,17 @@ 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, 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. +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 +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: @@ -123,15 +127,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 @@ -143,8 +148,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: @@ -167,7 +174,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 @@ -184,18 +193,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. @@ -209,7 +221,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. @@ -223,7 +235,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 new file mode 100644 index 0000000..af74242 --- /dev/null +++ b/tools/shots/README.md @@ -0,0 +1,119 @@ +# 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. + +- `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 + 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. + +## 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. + +| 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. + +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 +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, 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 +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 and the Cell position is known. 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. diff --git a/tools/shots/export.js b/tools/shots/export.js new file mode 100644 index 0000000..dbc5554 --- /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(p); +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/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/index.html b/tools/shots/index.html new file mode 100644 index 0000000..c99d193 --- /dev/null +++ b/tools/shots/index.html @@ -0,0 +1,196 @@ + + + + +BIOBUZZ shot table + + + + + +
+

Ball

+ + +

Actions

+ + + + +

+
+
+ +
+ + + + +
+
+
+
+
+ + + diff --git a/tools/shots/inputs.json b/tools/shots/inputs.json new file mode 100644 index 0000000..9c7f37b --- /dev/null +++ b/tools/shots/inputs.json @@ -0,0 +1,16 @@ +{ + "ball": "POLLEN", + "ballDiameterM": 0.0711, + "ballMassKg": 0.0249, + "dragCd": 0.45, + "launchAngleDeg": 80, + "exitHeightM": 0.40, + "exitSpeedPerRpm": 0.0023, + "shooterOffsetM": 0.15, + "rpmToleranceRpm": 50, + "lipHeightM": 1.359, + "openingWidthM": 0.508, + "openingLengthM": 0.356, + "openingTiltDeg": 60, + "marginM": 0.02 +} diff --git a/tools/shots/shots.js b/tools/shots/shots.js new file mode 100644 index 0000000..2f71c43 --- /dev/null +++ b/tools/shots/shots.js @@ -0,0 +1,205 @@ +// 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: 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 + 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: 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.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, + }; + + 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) 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; + let x = 0, y = p.exitHeightM, vx = v0 * Math.cos(a) + radialVelMps, vy = v0 * Math.sin(a), t = 0; + 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; + 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); + 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 { 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. + 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' + + ' /** 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' + + ' 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, 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({}, 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]) { + 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, 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]; + 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, fly: fly, land: land, simulate: simulate, band: band, solveGrid: solveGrid, toJava: toJava, selfCheck: selfCheck }; +});