Add raw pinpoint pose logging

wheelerrobotics · Nov 18, 2024 · a70c9d4 · a70c9d4
1 parent 80b1f9f
commit a70c9d4
Showing 1 changed file with 20 additions and 0 deletions.
diff --git 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);
+        }
+    }
+
+
+
 }