From d81a5f1994e80e204310e6270525531375ed8ec1 Mon Sep 17 00:00:00 2001 From: Aryan Mathur <134450794+aryanmathur1@users.noreply.github.com> Date: Mon, 20 Jul 2026 15:08:13 -0400 Subject: [PATCH 1/2] Create PoseRecorder.java By Metal Magic FTC 23362 We added a TeleOp for authoring field coordinates by moving the robot instead of measuring the field by hand. Set a starting pose in code, then drag or drive the robot around the field and press a button to capture the robot's current pose. Captures accumulate in a numbered, copy-pasteable list on the Driver Station and stream to the coding computer via adb logcat, so they drop straight into path code. --- .../teamcode/pedroPathing/PoseRecorder.java | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java new file mode 100644 index 000000000000..37064816895e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java @@ -0,0 +1,106 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the PoseRecorder OpMode. It lets you author field coordinates by moving the robot + * instead of measuring the field by hand. Set a starting pose below, run the OpMode, then drive + * the robot with gamepad 1 OR just push it around by hand (odometry tracks it either way). Press + * a button to capture wherever the robot currently is. + * + *

Captured poses are shown on the Driver Station telemetry as a numbered, copy-paste-ready list + * of {@code new Pose(x, y, Math.toRadians(deg))} lines, and each capture is also logged via + * {@link RobotLog} under the tag "PoseRecorder" so it reaches the coding laptop through + * {@code adb logcat}. + * + *

Controls (gamepad 1): + *

+ * + *

To use, edit {@link #startingPose} below, and if needed the follower-creation line in + * {@link #init()}. Captures are in the Follower's native Pedro coordinate frame. + */ +@TeleOp(name = "Pose Recorder", group = "Pedro Pathing") +public class PoseRecorder extends OpMode { + /** Tag used for {@link RobotLog} so captures show up in {@code adb logcat}. */ + private static final String TAG = "PoseRecorder"; + + /** Edit this to set where the robot starts on the field. */ + public Pose startingPose = new Pose(72, 72, Math.toRadians(0)); + + private Follower follower; + + private final List capturedPoses = new ArrayList<>(); + + @Override + public void init() { + // Edit this line if you build your Follower differently. + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(startingPose); + } + + @Override + public void init_loop() { + telemetry.addLine("Pose Recorder ready."); + telemetry.addLine("Drive with gamepad 1 or push the robot by hand."); + telemetry.addLine("A = capture, B = undo, Back + Y = clear."); + telemetry.update(); + follower.update(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + } + + @Override + public void loop() { + // Let the robot be driven; it can also just be pushed by hand (odometry tracks it either way). + follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); + follower.update(); + + if (gamepad1.aWasPressed()) { + Pose captured = follower.getPose(); + capturedPoses.add(captured); + RobotLog.ii(TAG, "Captured #%d: %s", capturedPoses.size(), formatPose(captured)); + } + + if (gamepad1.bWasPressed() && !capturedPoses.isEmpty()) { + Pose removed = capturedPoses.remove(capturedPoses.size() - 1); + RobotLog.ii(TAG, "Removed last capture: %s", formatPose(removed)); + } + + // Back + Y guard combo so the list isn't cleared by accident. + if (gamepad1.back && gamepad1.yWasPressed()) { + capturedPoses.clear(); + RobotLog.ii(TAG, "Cleared all captures."); + } + + Pose current = follower.getPose(); + telemetry.addLine("A = capture, B = undo, Back + Y = clear."); + telemetry.addData("Current pose", formatPose(current)); + telemetry.addLine(); + telemetry.addData("Captured poses", capturedPoses.size()); + for (int i = 0; i < capturedPoses.size(); i++) { + telemetry.addData(String.valueOf(i), formatPose(capturedPoses.get(i))); + } + telemetry.update(); + } + + /** Formats a Pose as a copy-paste-ready {@code new Pose(x, y, Math.toRadians(deg))} line. */ + private String formatPose(Pose pose) { + return String.format( + "new Pose(%.2f, %.2f, Math.toRadians(%.2f))", + pose.getX(), pose.getY(), Math.toDegrees(pose.getHeading())); + } +} From d4ce698c755b5037cd3f3abfa3b0b8cb380a8164 Mon Sep 17 00:00:00 2001 From: Aryan Mathur <134450794+aryanmathur1@users.noreply.github.com> Date: Tue, 21 Jul 2026 12:16:26 -0400 Subject: [PATCH 2/2] Update PoseRecorder.java Changed saving from logcat to: Buttons are now: A capture, B undo, X save, Back+Y clear. Retrieving the file (documented in the class Javadoc): Android Studio teams: adb pull /sdcard/FIRST/pose_recorder.txt OnBot Java / no adb: plug the Control Hub or RC phone into the computer over USB and browse to the FIRST folder (the exact on-robot path is shown live in telemetry). --- .../teamcode/pedroPathing/PoseRecorder.java | 75 +++++++++++++++---- 1 file changed, 59 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java index 37064816895e..b2661284f193 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/PoseRecorder.java @@ -4,8 +4,11 @@ import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.util.RobotLog; +import com.qualcomm.robotcore.util.ReadWriteFile; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.File; import java.util.ArrayList; import java.util.List; @@ -16,24 +19,37 @@ * a button to capture wherever the robot currently is. * *

Captured poses are shown on the Driver Station telemetry as a numbered, copy-paste-ready list - * of {@code new Pose(x, y, Math.toRadians(deg))} lines, and each capture is also logged via - * {@link RobotLog} under the tag "PoseRecorder" so it reaches the coding laptop through - * {@code adb logcat}. + * of {@code new Pose(x, y, Math.toRadians(deg))} lines. Press X to write the whole list to a text + * file on the Robot Controller (see {@link #OUTPUT_PATH}) so you can pull it onto your coding + * laptop and drop the lines straight into your path code. + * + *

Getting the file onto your computer: + *

* *

Controls (gamepad 1): *

* - *

To use, edit {@link #startingPose} below, and if needed the follower-creation line in - * {@link #init()}. Captures are in the Follower's native Pedro coordinate frame. + *

To use, edit {@link #startingPose} below, and if needed {@link #OUTPUT_PATH} and the + * follower-creation line in {@link #init()}. Captures are in the Follower's native Pedro + * coordinate frame. */ @TeleOp(name = "Pose Recorder", group = "Pedro Pathing") public class PoseRecorder extends OpMode { - /** Tag used for {@link RobotLog} so captures show up in {@code adb logcat}. */ - private static final String TAG = "PoseRecorder"; + /** + * Where captured poses are written when you press X. Defaults to the FTC "FIRST" folder on the + * Robot Controller, which every team already has and can reach over adb or USB. Edit this to + * put the file wherever you like (any absolute path the app can write to). + */ + public String OUTPUT_PATH = new File(AppUtil.FIRST_FOLDER, "pose_recorder.txt").getPath(); /** Edit this to set where the robot starts on the field. */ public Pose startingPose = new Pose(72, 72, Math.toRadians(0)); @@ -42,6 +58,8 @@ public class PoseRecorder extends OpMode { private final List capturedPoses = new ArrayList<>(); + private String saveStatus = "Nothing saved yet."; + @Override public void init() { // Edit this line if you build your Follower differently. @@ -53,7 +71,8 @@ public void init() { public void init_loop() { telemetry.addLine("Pose Recorder ready."); telemetry.addLine("Drive with gamepad 1 or push the robot by hand."); - telemetry.addLine("A = capture, B = undo, Back + Y = clear."); + telemetry.addLine("A = capture, B = undo, X = save, Back + Y = clear."); + telemetry.addData("Save file", OUTPUT_PATH); telemetry.update(); follower.update(); } @@ -70,33 +89,57 @@ public void loop() { follower.update(); if (gamepad1.aWasPressed()) { - Pose captured = follower.getPose(); - capturedPoses.add(captured); - RobotLog.ii(TAG, "Captured #%d: %s", capturedPoses.size(), formatPose(captured)); + capturedPoses.add(follower.getPose()); } if (gamepad1.bWasPressed() && !capturedPoses.isEmpty()) { - Pose removed = capturedPoses.remove(capturedPoses.size() - 1); - RobotLog.ii(TAG, "Removed last capture: %s", formatPose(removed)); + capturedPoses.remove(capturedPoses.size() - 1); + } + + if (gamepad1.xWasPressed()) { + saveToFile(); } // Back + Y guard combo so the list isn't cleared by accident. if (gamepad1.back && gamepad1.yWasPressed()) { capturedPoses.clear(); - RobotLog.ii(TAG, "Cleared all captures."); } Pose current = follower.getPose(); - telemetry.addLine("A = capture, B = undo, Back + Y = clear."); + telemetry.addLine("A = capture, B = undo, X = save, Back + Y = clear."); telemetry.addData("Current pose", formatPose(current)); telemetry.addLine(); telemetry.addData("Captured poses", capturedPoses.size()); for (int i = 0; i < capturedPoses.size(); i++) { telemetry.addData(String.valueOf(i), formatPose(capturedPoses.get(i))); } + telemetry.addLine(); + telemetry.addData("Save file", OUTPUT_PATH); + telemetry.addData("Save status", saveStatus); telemetry.update(); } + /** Writes every captured pose to {@link #OUTPUT_PATH} as copy-paste-ready lines. */ + private void saveToFile() { + StringBuilder sb = new StringBuilder(); + sb.append("// Pose Recorder capture — ").append(capturedPoses.size()).append(" pose(s)\n"); + for (Pose pose : capturedPoses) { + sb.append(formatPose(pose)).append("\n"); + } + + File file = new File(OUTPUT_PATH); + try { + File parent = file.getParentFile(); + if (parent != null) { + parent.mkdirs(); + } + ReadWriteFile.writeFileOrThrow(file, sb.toString()); + saveStatus = "Saved " + capturedPoses.size() + " pose(s) to " + file.getPath(); + } catch (Exception e) { + saveStatus = "Save FAILED: " + e.getMessage(); + } + } + /** Formats a Pose as a copy-paste-ready {@code new Pose(x, y, Math.toRadians(deg))} line. */ private String formatPose(Pose pose) { return String.format(