From f4942715d984666bd8d1c0fb57fdaba1b0cec02b Mon Sep 17 00:00:00 2001
From: Logan-Nash <lnash18145a@gmail.com>
Date: Tue, 10 Dec 2024 10:40:22 -0500
Subject: [PATCH] Fixed loop times on OTOS localizer. Updated the pinpoint to
 the latest driver

---
 .../pedroPathing/localization/GoBildaPinpointDriver.java  | 7 +++++++
 .../localization/localizers/OTOSLocalizer.java            | 8 +++++---
 2 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java
index a2bc9d8..882ed56 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java
@@ -314,9 +314,16 @@ public class GoBildaPinpointDriver extends I2cDeviceSynchDevice<I2cDeviceSynchSi
      * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left
      */
     public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){
+        if (xEncoder == EncoderDirection.FORWARD){
+            writeInt(Register.DEVICE_CONTROL,1<<5);
+        }
         if (xEncoder == EncoderDirection.REVERSED) {
             writeInt(Register.DEVICE_CONTROL,1<<4);
         }
+
+        if (yEncoder == EncoderDirection.FORWARD){
+            writeInt(Register.DEVICE_CONTROL,1<<3);
+        }
         if (yEncoder == EncoderDirection.REVERSED){
             writeInt(Register.DEVICE_CONTROL,1<<2);
         }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
index a700e3b..0f309a5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
@@ -108,10 +108,12 @@ public class OTOSLocalizer extends Localizer {
      */
     @Override
     public Pose getPose() {
-        SparkFunOTOS.Pose2D rawPose = otos.getPosition();
-        Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);
+        Pose pose = new Pose(otosPose.x, otosPose.y, otosPose.h);
 
-        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
+        Vector vec = pose.getVector();
+        vec.rotateVector(startPose.getHeading());
+
+        return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
     }
 
     /**