From 9711a268e5b798bf3be4598aea55f0ad91604414 Mon Sep 17 00:00:00 2001
From: Anyi Lin <anyo3223@gmail.com>
Date: Thu, 21 Nov 2024 17:09:44 -0500
Subject: [PATCH] hopefully fixed the PinpointLocalizer

---
 .../localizers/PinpointLocalizer.java         | 24 +++++++++----------
 1 file changed, 11 insertions(+), 13 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
index 0d6ff4b..58c4746 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
@@ -44,7 +44,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
  */
 public class PinpointLocalizer extends Localizer {
     private HardwareMap hardwareMap;
-    private Pose startPose;
     private GoBildaPinpointDriver odo;
     private double previousHeading;
     private double totalHeading;
@@ -84,7 +83,7 @@ public class PinpointLocalizer extends Localizer {
 
         setStartPose(setStartPose);
         totalHeading = 0;
-        previousHeading = startPose.getHeading();
+        previousHeading = setStartPose.getHeading();
 
         resetPinpoint();
     }
@@ -97,9 +96,7 @@ public class PinpointLocalizer extends Localizer {
     @Override
     public Pose getPose() {
         Pose2D rawPose = odo.getPosition();
-        Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
-
-        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
+        return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
     }
 
     /**
@@ -127,13 +124,15 @@ public class PinpointLocalizer extends Localizer {
     }
 
     /**
-     * This sets the start pose. Changing the start pose should move the robot as if all its
-     * previous movements were displacing it from its new start pose.
+     * This sets the start pose. Since nobody should be using this after the robot has begun moving,
+     * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose).
      *
      * @param setStart the new start pose
      */
     @Override
-    public void setStartPose(Pose setStart) {startPose = setStart;}
+    public void setStartPose(Pose setStart) {
+        odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading()));
+    }
 
     /**
      * This sets the current pose estimate. Changing this should just change the robot's current
@@ -143,9 +142,8 @@ public class PinpointLocalizer extends Localizer {
      */
     @Override
     public void setPose(Pose setPose) {
-    resetPinpoint();
-    Pose setPinpointPose = MathFunctions.subtractPoses(setPose, startPose);
-    odo.setPosition(new Pose2D(DistanceUnit.INCH, setPinpointPose.getX(), setPinpointPose.getY(), AngleUnit.RADIANS, setPinpointPose.getHeading()));
+        resetPinpoint();
+        odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading()));
     }
 
     /**
@@ -154,8 +152,8 @@ public class PinpointLocalizer extends Localizer {
     @Override
     public void update() {
         odo.update();
-    totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(),previousHeading);
-    previousHeading = odo.getHeading();
+        totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(), previousHeading);
+        previousHeading = odo.getHeading();
     }
 
     /**