added functionality to IMU methods in PoseUpdater

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization;
 
+import com.qualcomm.robotcore.hardware.IMU;
+
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 
 /**
@@ -91,4 +93,13 @@ public abstract class Localizer {
      * This resets the IMU of the localizer, if applicable.
      */
     public abstract void resetIMU();
+
+    /**
+     * This is overridden to return the IMU, if there is one.
+     *
+     * @return returns the IMU if it exists
+     */
+    public IMU getIMU() {
+        return null;
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java

@@ -60,6 +60,7 @@ public class PoseUpdater {
         }
 
         this.localizer = localizer;
+        imu = localizer.getIMU();
     }
 
     /**
@@ -300,8 +301,10 @@ public class PoseUpdater {
      * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset.
      */
     public void resetHeadingToIMU() {
+        if (imu != null) {
             localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
         }
+    }
 
     /**
      * This resets the heading of the robot to the IMU's heading, using offsets instead of Road
@@ -309,8 +312,10 @@ public class PoseUpdater {
      * method.
      */
     public void resetHeadingToIMUWithOffsets() {
+        if (imu != null) {
             setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
         }
+    }
 
     /**
      * This returns the IMU heading normalized to be between [0, 2 PI] radians.
@@ -318,8 +323,11 @@ public class PoseUpdater {
      * @return returns the normalized IMU heading.
      */
     public double getNormalizedIMUHeading() {
+        if (imu != null) {
             return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
         }
+        return 0;
+    }
 
     /**
      * This returns the total number of radians the robot has turned.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java

@@ -313,4 +313,14 @@ public class ThreeWheelIMULocalizer extends Localizer {
     public void resetIMU() {
         imu.resetYaw();
     }
+
+    /**
+     * This is returns the IMU.
+     *
+     * @return returns the IMU
+     */
+    @Override
+    public IMU getIMU() {
+        return imu;
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java

@@ -296,4 +296,14 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w
     public void resetIMU() {
         imu.resetYaw();
     }
+
+    /**
+     * This is returns the IMU.
+     *
+     * @return returns the IMU
+     */
+    @Override
+    public IMU getIMU() {
+        return imu;
+    }
 }