From a70c9d441d4169078cd567fed0750b79b8a36f1e Mon Sep 17 00:00:00 2001
From: j5155 <54331556+j5155@users.noreply.github.com>
Date: Mon, 7 Oct 2024 10:17:50 -0800
Subject: [PATCH] Add raw pinpoint pose logging

---
 .../ftc/teamcode/PinpointDrive.java           | 20 +++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java
index 4dd471c4a7fc..124e407d8677 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java
@@ -8,7 +8,9 @@
 import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
 import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+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.messages.PoseMessage;
 
 /**
@@ -113,9 +115,27 @@ public PoseVelocity2d updatePoseEstimate() {
         }
 
         FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
+        FlightRecorder.write("PINPOINT_RAW_POSE",new FTCPoseMessage(pinpoint.getPosition()));
 
         return pinpoint.getVelocityRR();
     }
 
 
+    // for debug logging
+    public static final class FTCPoseMessage {
+        public long timestamp;
+        public double x;
+        public double y;
+        public double heading;
+
+        public FTCPoseMessage(Pose2D pose) {
+            this.timestamp = System.nanoTime();
+            this.x = pose.getX(DistanceUnit.INCH);
+            this.y = pose.getY(DistanceUnit.INCH);
+            this.heading = pose.getHeading(AngleUnit.RADIANS);
+        }
+    }
+
+
+
 }