diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index 7182215..ddccf84 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -272,7 +272,6 @@ public class SwerveDrive implements AutoCloseable
 
       // register the drivetrain simulation
       SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive);
-
       simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation());
       imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
     } else
@@ -289,8 +288,11 @@ public class SwerveDrive implements AutoCloseable
             getYaw(),
             getModulePositions(),
             startingPose); // x,y,heading in radians; Vision measurement std dev, higher=less weight
-
-    zeroGyro();
+//
+//    Rotation3d currentGyro = imuReadingCache.getValue();
+//    double offset = currentGyro.getZ() +
+//                    startingPose.getRotation().getRadians();
+//    setGyroOffset(new Rotation3d(currentGyro.getX(), currentGyro.getY(), offset));
 
     // Initialize Telemetry
     if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
@@ -335,11 +337,13 @@ public class SwerveDrive implements AutoCloseable
   }
 
   @Override
-  public void close() {
+  public void close()
+  {
     imu.close();
     tunerXRecommendation.close();
 
-    for (var module : swerveModules) {
+    for (var module : swerveModules)
+    {
       module.close();
     }
   }
@@ -393,7 +397,10 @@ public class SwerveDrive implements AutoCloseable
   public void setOdometryPeriod(double period)
   {
     odometryThread.stop();
-    SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
+    }
     odometryThread.startPeriodic(period);
   }
 
@@ -403,7 +410,10 @@ public class SwerveDrive implements AutoCloseable
   public void stopOdometryThread()
   {
     odometryThread.stop();
-    SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5);
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5);
+    }
   }
 
   /**
@@ -778,7 +788,7 @@ public class SwerveDrive implements AutoCloseable
     {
       module.applyStateOptimizations(states[module.moduleNumber]);
       module.applyAntiJitter(states[module.moduleNumber], false);
-      
+
       // from the module configuration, obtain necessary information to calculate feed-forward
       // Warning: Will not work well if motor is not what we are expecting.
       // Warning: Should replace module.getDriveMotor().simMotor with expected motor type first.
@@ -898,7 +908,7 @@ public class SwerveDrive implements AutoCloseable
    * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
    * keep working.
    *
-   * @param pose The pose to set the odometry to
+   * @param pose The pose to set the odometry to. Field relative, blue-origin where 0deg is facing towards RED alliance.
    */
   public void resetOdometry(Pose2d pose)
   {
@@ -987,7 +997,7 @@ public class SwerveDrive implements AutoCloseable
   }
 
   /**
-   * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
+   * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0 (red alliance station).
    */
   public void zeroGyro()
   {
diff --git a/swervelib/SwerveInputStream.java b/swervelib/SwerveInputStream.java
index ba366d3..c01ae92 100644
--- a/swervelib/SwerveInputStream.java
+++ b/swervelib/SwerveInputStream.java
@@ -1,11 +1,16 @@
 package swervelib;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.XboxController;
@@ -57,99 +62,95 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
   /**
    * Rotation supplier as angular velocity.
    */
-  private       Optional<DoubleSupplier>        controllerOmega               = Optional.empty();
+  private Optional<DoubleSupplier>        controllerOmega                     = Optional.empty();
   /**
    * Controller supplier as heading.
    */
-  private       Optional<DoubleSupplier>        controllerHeadingX            = Optional.empty();
+  private Optional<DoubleSupplier>        controllerHeadingX                  = Optional.empty();
   /**
    * Controller supplier as heading.
    */
-  private       Optional<DoubleSupplier>        controllerHeadingY            = Optional.empty();
+  private Optional<DoubleSupplier>        controllerHeadingY                  = Optional.empty();
   /**
    * Axis deadband for the controller.
    */
-  private       Optional<Double>                axisDeadband                  = Optional.empty();
+  private Optional<Double>                axisDeadband                        = Optional.empty();
   /**
    * Translational axis scalar value, should be between (0, 1].
    */
-  private       Optional<Double>                translationAxisScale          = Optional.empty();
+  private Optional<Double>                translationAxisScale                = Optional.empty();
   /**
    * Angular velocity axis scalar value, should be between (0, 1]
    */
-  private       Optional<Double>                omegaAxisScale                = Optional.empty();
+  private Optional<Double>                omegaAxisScale                      = Optional.empty();
   /**
    * Target to aim at.
    */
-  private       Optional<Pose2d>                aimTarget                     = Optional.empty();
+  private Optional<Pose2d>                aimTarget                           = Optional.empty();
   /**
    * Target {@link Supplier<Pose2d>} to drive towards when driveToPose is enabled.
    */
-  private       Optional<Supplier<Pose2d>>      driveToPose                   = Optional.empty();
+  private Optional<Supplier<Pose2d>>      driveToPose                         = Optional.empty();
   /**
-   * {@link ProfiledPIDController} for the X translation while driving to a pose. Units are m/s
+   * {@link ProfiledPIDController} for the translation while driving to a pose. Units are m/s
    */
-  private       Optional<ProfiledPIDController> driveToPoseXPIDController     = Optional.empty();
-  /**
-   * {@link ProfiledPIDController} for the Y translation while driving to a pose. Units are m/s
-   */
-  private       Optional<ProfiledPIDController> driveToPoseYPIDController     = Optional.empty();
+  private Optional<ProfiledPIDController> driveToPoseTranslationPIDController = Optional.empty();
   /**
    * {@link ProfiledPIDController} for the Rotational axis while driving to a pose. Units are m/s
    */
-  private       Optional<ProfiledPIDController> driveToPoseOmegaPIDController = Optional.empty();
+  private Optional<ProfiledPIDController> driveToPoseOmegaPIDController       = Optional.empty();
   /**
    * Output {@link ChassisSpeeds} based on heading while this is True.
    */
-  private       Optional<BooleanSupplier>       headingEnabled                = Optional.empty();
+  private Optional<BooleanSupplier>       headingEnabled                      = Optional.empty();
   /**
    * Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY}
    */
-  private       Optional<Rotation2d>            lockedHeading                 = Optional.empty();
+  private Optional<Rotation2d>            lockedHeading                       = Optional.empty();
   /**
    * Output {@link ChassisSpeeds} based on aim while this is True.
    */
-  private       Optional<BooleanSupplier>       aimEnabled                    = Optional.empty();
+  private Optional<BooleanSupplier>       aimEnabled                          = Optional.empty();
   /**
    * Output {@link ChassisSpeeds} to move to a specific {@link Pose2d}.
    */
-  private       Optional<BooleanSupplier>       driveToPoseEnabled            = Optional.empty();
+  private Optional<BooleanSupplier>       driveToPoseEnabled                  = Optional.empty();
   /**
    * Maintain current heading and drive without rotating, ideally.
    */
-  private       Optional<BooleanSupplier>       translationOnlyEnabled        = Optional.empty();
+  private Optional<BooleanSupplier>       translationOnlyEnabled              = Optional.empty();
   /**
    * Cube the translation magnitude from the controller.
    */
-  private       Optional<BooleanSupplier>       translationCube               = Optional.empty();
+  private Optional<BooleanSupplier>       translationCube                     = Optional.empty();
   /**
    * Cube the angular velocity axis from the controller.
    */
-  private       Optional<BooleanSupplier>       omegaCube                     = Optional.empty();
+  private Optional<BooleanSupplier>       omegaCube                           = Optional.empty();
   /**
    * Robot relative oriented output expected.
    */
-  private       Optional<BooleanSupplier>       robotRelative                 = Optional.empty();
+  private Optional<BooleanSupplier>       robotRelative                       = Optional.empty();
   /**
    * Field oriented chassis output is relative to your current alliance.
    */
-  private       Optional<BooleanSupplier>       allianceRelative              = Optional.empty();
+  private Optional<BooleanSupplier>       allianceRelative                    = Optional.empty();
   /**
    * Heading offset enable state.
    */
-  private       Optional<BooleanSupplier>       headingOffsetEnabled          = Optional.empty();
+  private Optional<BooleanSupplier>       translationHeadingOffsetEnabled     = Optional.empty();
   /**
    * Heading offset to apply during heading based control.
    */
-  private       Optional<Rotation2d>            headingOffset                 = Optional.empty();
+  private Optional<Rotation2d>            translationHeadingOffset            = Optional.empty();
   /**
    * {@link SwerveController} for simple control over heading.
    */
-  private       SwerveController                swerveController              = null;
+  private SwerveController                swerveController                    = null;
   /**
    * Current {@link SwerveInputMode} to use.
    */
-  private       SwerveInputMode                 currentMode                   = SwerveInputMode.ANGULAR_VELOCITY;
+  private SwerveInputMode                 currentMode                         = SwerveInputMode.ANGULAR_VELOCITY;
 
 
   /**
@@ -225,8 +226,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     newStream.translationAxisScale = translationAxisScale;
     newStream.omegaAxisScale = omegaAxisScale;
     newStream.driveToPose = driveToPose;
-    newStream.driveToPoseXPIDController = driveToPoseXPIDController;
-    newStream.driveToPoseYPIDController = driveToPoseYPIDController;
+    newStream.driveToPoseTranslationPIDController = driveToPoseTranslationPIDController;
     newStream.driveToPoseOmegaPIDController = driveToPoseOmegaPIDController;
     newStream.aimTarget = aimTarget;
     newStream.headingEnabled = headingEnabled;
@@ -240,8 +240,8 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     newStream.translationCube = translationCube;
     newStream.robotRelative = robotRelative;
     newStream.allianceRelative = allianceRelative;
-    newStream.headingOffsetEnabled = headingOffsetEnabled;
-    newStream.headingOffset = headingOffset;
+    newStream.translationHeadingOffsetEnabled = translationHeadingOffsetEnabled;
+    newStream.translationHeadingOffset = translationHeadingOffset;
     return newStream;
   }
 
@@ -273,37 +273,23 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
    * Drive to a given pose with the provided {@link ProfiledPIDController}s
    *
    * @param pose               {@link Supplier<Pose2d>} for ease of use.
-   * @param xPIDController     PID controller for the X axis, units are m/s.
-   * @param yPIDController     PID controller for the Y axis, units are m/s.
+   * @param xPIDController     PID controller for the translational axis, units are m/s.
    * @param omegaPIDController PID Controller for rotational axis, units are rad/s.
    * @return self
    */
   public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController xPIDController,
-                                       ProfiledPIDController yPIDController, ProfiledPIDController omegaPIDController)
+                                       ProfiledPIDController omegaPIDController)
   {
+    omegaPIDController.reset(swerveDrive.getPose().getRotation().getRadians());
+    xPIDController.reset(swerveDrive.getPose().getTranslation().getDistance(pose.get().getTranslation()));
+    omegaPIDController.enableContinuousInput(-Math.PI, Math.PI);
+    xPIDController.setGoal(new State(0, 0));
     driveToPose = Optional.of(pose);
-    driveToPoseXPIDController = Optional.of(xPIDController);
-    driveToPoseYPIDController = Optional.of(yPIDController);
+    driveToPoseTranslationPIDController = Optional.of(xPIDController);
     driveToPoseOmegaPIDController = Optional.of(omegaPIDController);
     return this;
   }
 
-  /**
-   * Drive to a given pose with the provided {@link ProfiledPIDController}s
-   *
-   * @param pose        {@link Supplier<Pose2d>} for ease of use.
-   * @param translation PID controller for the X and Y axis, units are m/s.
-   * @param rotation    PID Controller for rotational axis, units are rad/s.
-   * @return self
-   */
-  public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController translation,
-                                       ProfiledPIDController rotation)
-  {
-    return driveToPose(pose, translation, new ProfiledPIDController(translation.getP(),
-                                                                    translation.getI(),
-                                                                    translation.getD(),
-                                                                    translation.getConstraints()), rotation);
-  }
 
   /**
    * Enable driving to the target pose.
@@ -326,6 +312,11 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
   public SwerveInputStream driveToPoseEnabled(boolean enabled)
   {
     driveToPoseEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
+    Pose2d swervePose = swerveDrive.getPose();
+//    driveToPoseXPIDController.ifPresent(profiledPIDController -> profiledPIDController.reset(swervePose.getX()));
+//    driveToPoseYPIDController.ifPresent(profiledPIDController -> profiledPIDController.reset(swervePose.getY()));
+//    driveToPoseOmegaPIDController.ifPresent(profiledPIDController -> profiledPIDController.reset(swervePose.getRotation()
+//                                                                                                           .getRadians()));
     return this;
   }
 
@@ -336,9 +327,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
    * @param enabled Enable state
    * @return self
    */
-  public SwerveInputStream headingOffset(BooleanSupplier enabled)
+  public SwerveInputStream translationHeadingOffset(BooleanSupplier enabled)
   {
-    headingOffsetEnabled = Optional.of(enabled);
+    translationHeadingOffsetEnabled = Optional.of(enabled);
     return this;
   }
 
@@ -348,9 +339,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
    * @param enabled Enable state
    * @return self
    */
-  public SwerveInputStream headingOffset(boolean enabled)
+  public SwerveInputStream translationHeadingOffset(boolean enabled)
   {
-    headingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
+    translationHeadingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
     return this;
   }
 
@@ -360,9 +351,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
    * @param angle {@link Rotation2d} offset to apply
    * @return self
    */
-  public SwerveInputStream headingOffset(Rotation2d angle)
+  public SwerveInputStream translationHeadingOffset(Rotation2d angle)
   {
-    headingOffset = Optional.of(angle);
+    translationHeadingOffset = Optional.of(angle);
     return this;
   }
 
@@ -613,11 +604,11 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     {
       if (driveToPose.isPresent())
       {
-        if (driveToPoseOmegaPIDController.isPresent() && driveToPoseXPIDController.isPresent() &&
-            driveToPoseYPIDController.isPresent())
+        if (driveToPoseOmegaPIDController.isPresent() && driveToPoseTranslationPIDController.isPresent())
         {
           return SwerveInputMode.DRIVE_TO_POSE;
         }
+        System.out.println("Drive to pose present");
         DriverStation.reportError("Drive to pose not supplied with pid controllers.", false);
       }
       DriverStation.reportError("Drive to pose enabled without supplier present.", false);
@@ -671,11 +662,15 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         lockedHeading = Optional.empty();
         break;
       }
-      case ANGULAR_VELOCITY, HEADING, AIM, DRIVE_TO_POSE ->
+      case ANGULAR_VELOCITY, HEADING, AIM ->
       {
         // Do nothing
         break;
       }
+      case DRIVE_TO_POSE ->
+      {
+        break;
+      }
     }
 
     // Transitioning to new mode
@@ -686,7 +681,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         lockedHeading = Optional.of(swerveDrive.getOdometryHeading());
         break;
       }
-      case ANGULAR_VELOCITY, DRIVE_TO_POSE ->
+      case ANGULAR_VELOCITY ->
       {
         if (swerveDrive.headingCorrection)
         {
@@ -699,6 +694,13 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         // Do nothing
         break;
       }
+      case DRIVE_TO_POSE ->
+      {
+        if (swerveDrive.headingCorrection)
+        {
+          swerveDrive.setHeadingCorrection(false);
+        }
+      }
     }
   }
 
@@ -807,6 +809,10 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     {
       if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
       {
+        if (driveToPoseEnabled.isPresent() && driveToPoseEnabled.get().getAsBoolean())
+        {
+          return fieldRelativeTranslation;
+        }
         throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!");
       }
       if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
@@ -818,43 +824,49 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
   }
 
   /**
-   * Apply alliance aware translation which flips the {@link Rotation2d} if the robot is on the Blue alliance.
+   * Adds offset to translation if one is set.
    *
-   * @param fieldRelativeRotation Field-relative {@link Rotation2d} to flip.
-   * @return Alliance-oriented {@link Rotation2d}
+   * @param speeds {@link ChassisSpeeds} to offset
+   * @return Offsetted {@link ChassisSpeeds}
    */
-  private Rotation2d applyAllianceAwareRotation(Rotation2d fieldRelativeRotation)
+  private ChassisSpeeds applyTranslationHeadingOffset(ChassisSpeeds speeds)
   {
-    if (allianceRelative.isPresent() && allianceRelative.get().getAsBoolean())
+    if (translationHeadingOffsetEnabled.isPresent() && translationHeadingOffsetEnabled.get().getAsBoolean())
     {
-      if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
+      if (translationHeadingOffset.isPresent())
       {
-        throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!");
-      }
-      if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
-      {
-        return fieldRelativeRotation.rotateBy(Rotation2d.k180deg);
+        Translation2d speedsTranslation = new Translation2d(speeds.vxMetersPerSecond,
+                                                            speeds.vyMetersPerSecond).rotateBy(translationHeadingOffset.get());
+        return new ChassisSpeeds(speedsTranslation.getX(), speedsTranslation.getY(), speeds.omegaRadiansPerSecond);
       }
     }
-    return fieldRelativeRotation;
+    return speeds;
   }
 
   /**
-   * Adds offset to rotation if one is set.
+   * When the {@link SwerveInputStream} is in {@link SwerveInputMode#DRIVE_TO_POSE} this function will return if the
+   * robot is at the desired pose within the defined tolerance.
    *
-   * @param fieldRelativeRotation Field-relative {@link Rotation2d} to offset
-   * @return Offsetted {@link Rotation2d}
+   * @param toleranceMeters Tolerance in meters.
+   * @return At target pose, true if current mode is not {@link SwerveInputMode#DRIVE_TO_POSE} and no pose supplier has
+   * been given.
    */
-  private Rotation2d applyHeadingOffset(Rotation2d fieldRelativeRotation)
+  public boolean atTargetPose(double toleranceMeters)
   {
-    if (headingOffsetEnabled.isPresent() && headingOffsetEnabled.get().getAsBoolean())
+    if (currentMode != SwerveInputMode.DRIVE_TO_POSE)
     {
-      if (headingOffset.isPresent())
+      DriverStation.reportError("SwerveInputStream.atTargetPose called while not set to DriveToPose.", false);
+      if (!driveToPose.isPresent())
       {
-        return fieldRelativeRotation.rotateBy(headingOffset.get());
+        return true;
       }
     }
-    return fieldRelativeRotation;
+    if (driveToPose.isPresent())
+    {
+      Pose2d targetPose = driveToPose.get().get();
+      return swerveDrive.getPose().getTranslation().getDistance(targetPose.getTranslation()) <= toleranceMeters;
+    }
+    return true;
   }
 
   /**
@@ -886,7 +898,6 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
     {
       swerveController = swerveDrive.getSwerveController();
     }
-
     switch (newMode)
     {
       case TRANSLATION_ONLY ->
@@ -907,14 +918,13 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
       case HEADING ->
       {
         omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
-                                                                  applyHeadingOffset(
-                                                                      applyAllianceAwareRotation(
-                                                                          Rotation2d.fromRadians(
-                                                                              swerveController.getJoystickAngle(
-                                                                                  controllerHeadingX.get()
-                                                                                                    .getAsDouble(),
-                                                                                  controllerHeadingY.get()
-                                                                                                    .getAsDouble())))).getRadians());
+                                                                  Rotation2d.fromRadians(
+                                                                                swerveController.getJoystickAngle(
+                                                                                    controllerHeadingX.get()
+                                                                                                      .getAsDouble(),
+                                                                                    controllerHeadingY.get()
+                                                                                                      .getAsDouble()))
+                                                                            .getRadians());
 
         // Prevent rotation if controller heading inputs are not past axisDeadband
         if (Math.abs(controllerHeadingX.get().getAsDouble()) + Math.abs(controllerHeadingY.get().getAsDouble()) <
@@ -922,7 +932,6 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
         {
           omegaRadiansPerSecond = 0;
         }
-
         speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
         break;
       }
@@ -937,20 +946,50 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
       }
       case DRIVE_TO_POSE ->
       {
-        Pose2d target = driveToPose.get().get();
-        Pose2d pose   = swerveDrive.getPose();
-        omegaRadiansPerSecond = driveToPoseOmegaPIDController.get().calculate(pose.getRotation()
-                                                                                  .getRadians(),
-                                                                              target.getRotation().getRadians());
-        speeds = new ChassisSpeeds(driveToPoseXPIDController.get().calculate(pose.getX(), target.getX()),
-                                   driveToPoseYPIDController.get().calculate(pose.getY(), target.getY()),
-                                   omegaRadiansPerSecond);
+        // Written by team 8865!
+        ProfiledPIDController translationPIDController = driveToPoseTranslationPIDController.get();
+        ProfiledPIDController rotationPIDController    = driveToPoseOmegaPIDController.get();
+        Pose2d                swervePoseSetpoint       = driveToPose.get().get();
+        Pose2d                robotPose                = swerveDrive.getPose();
+        Vector<N2>            robotVec                 = robotPose.getTranslation().toVector();
+        Vector<N2> targetPoseRelativeToRobotPose = swervePoseSetpoint.getTranslation().toVector().minus(
+            robotVec);
+        double distanceFromTarget = targetPoseRelativeToRobotPose.norm();
+
+        Vector<N2> traversalVector = new Vector(Nat.N2());
+        traversalVector.set(0, 0, targetPoseRelativeToRobotPose.get(0, 0));
+        traversalVector.set(1, 0, targetPoseRelativeToRobotPose.get(1, 0));
+        traversalVector = traversalVector.unit()
+                                         .times(-translationPIDController.calculate(distanceFromTarget, 0));
+
+        Vector<N2> robotForwardVec = robotPose.transformBy(new Transform2d(1, 0, new Rotation2d())).getTranslation()
+                                              .toVector().minus(robotVec);
+        Vector<N2> robotLateralVec = robotPose.transformBy(new Transform2d(0, 1, new Rotation2d())).getTranslation()
+                                              .toVector().minus(robotVec);
+
+        currentMode = newMode;
+        speeds = ChassisSpeeds.fromRobotRelativeSpeeds(new ChassisSpeeds(
+                                                           robotForwardVec.norm() * traversalVector.dot(robotForwardVec),
+                                                           robotLateralVec.norm() * traversalVector.dot(robotLateralVec),
+                                                           rotationPIDController.calculate(robotPose.getRotation().getRadians(),
+                                                                                           swervePoseSetpoint.getRotation().getRadians())),
+                                                       swerveDrive.getOdometryHeading());
+        double lerpDistance = robotPose.getTranslation().plus(new Translation2d(speeds.vxMetersPerSecond,
+                                                                                vyMetersPerSecond).times(0.02))
+                                       .getDistance(swervePoseSetpoint.getTranslation());
+        // Filter out incorrect ChassisSpeeds.
+        if (lerpDistance > distanceFromTarget)
+        {
+          speeds = new ChassisSpeeds(0, 0, 0);
+        }
+
+        return speeds;
       }
     }
 
     currentMode = newMode;
 
-    return applyRobotRelativeTranslation(speeds);
+    return applyTranslationHeadingOffset(applyRobotRelativeTranslation(speeds));
   }
 
   /**
diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java
index b9f6081..784a067 100644
--- a/swervelib/encoders/SparkMaxEncoderSwerve.java
+++ b/swervelib/encoders/SparkMaxEncoderSwerve.java
@@ -206,6 +206,18 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
   @Override
   public boolean setAbsoluteEncoderOffset(double offset)
   {
+    if (sparkMax instanceof SparkMaxSwerve)
+    {
+      SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
+      cfg.absoluteEncoder.zeroOffset(offset);
+      ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
+      return true;
+    } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+    {
+      SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
+      cfg.absoluteEncoder.zeroOffset(offset);
+      ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
+    }
     return false;
   }
 
diff --git a/swervelib/imu/ADIS16448Swerve.java b/swervelib/imu/ADIS16448Swerve.java
index fcea482..bbf966c 100644
--- a/swervelib/imu/ADIS16448Swerve.java
+++ b/swervelib/imu/ADIS16448Swerve.java
@@ -107,7 +107,7 @@ public class ADIS16448Swerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/imu/ADIS16470Swerve.java b/swervelib/imu/ADIS16470Swerve.java
index 0da8211..888d976 100644
--- a/swervelib/imu/ADIS16470Swerve.java
+++ b/swervelib/imu/ADIS16470Swerve.java
@@ -107,7 +107,7 @@ public class ADIS16470Swerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/imu/ADXRS450Swerve.java b/swervelib/imu/ADXRS450Swerve.java
index 85c7523..cbb390a 100644
--- a/swervelib/imu/ADXRS450Swerve.java
+++ b/swervelib/imu/ADXRS450Swerve.java
@@ -105,7 +105,7 @@ public class ADXRS450Swerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/imu/AnalogGyroSwerve.java b/swervelib/imu/AnalogGyroSwerve.java
index b5450f5..7c2d54b 100644
--- a/swervelib/imu/AnalogGyroSwerve.java
+++ b/swervelib/imu/AnalogGyroSwerve.java
@@ -112,7 +112,7 @@ public class AnalogGyroSwerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/imu/CanandgyroSwerve.java b/swervelib/imu/CanandgyroSwerve.java
index 7459d51..9991418 100644
--- a/swervelib/imu/CanandgyroSwerve.java
+++ b/swervelib/imu/CanandgyroSwerve.java
@@ -109,7 +109,7 @@ public class CanandgyroSwerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java
index b0f837b..8b8d073 100644
--- a/swervelib/imu/NavXSwerve.java
+++ b/swervelib/imu/NavXSwerve.java
@@ -20,7 +20,7 @@ public class NavXSwerve extends SwerveIMU
   /**
    * Mutable {@link MutAngularVelocity} for readings.
    */
-  private final MutAngularVelocity yawVel      = new MutAngularVelocity(0, 0, DegreesPerSecond);
+  private final MutAngularVelocity yawVel   = new MutAngularVelocity(0, 0, DegreesPerSecond);
   /**
    * NavX IMU.
    */
@@ -28,15 +28,15 @@ public class NavXSwerve extends SwerveIMU
   /**
    * Offset for the NavX.
    */
-  private       Rotation3d         offset      = new Rotation3d();
-  /**
-   * Inversion for the gyro
-   */
-  private       boolean            invertedIMU = false;
+  private       Rotation3d         offset   = new Rotation3d();
   /**
    * An {@link Alert} for if there is an error instantiating the NavX.
    */
   private       Alert              navXError;
+  /**
+   * Inversion state of the {@link AHRS}.
+   */
+  private       boolean            inverted = false;
 
   /**
    * Constructor for the NavX({@link AHRS}) swerve.
@@ -61,7 +61,8 @@ public class NavXSwerve extends SwerveIMU
   }
 
   @Override
-  public void close() {
+  public void close()
+  {
     imu.close();
   }
 
@@ -101,8 +102,8 @@ public class NavXSwerve extends SwerveIMU
    */
   public void setInverted(boolean invertIMU)
   {
-    invertedIMU = invertIMU;
-    setOffset(getRawRotation3d());
+    inverted = invertIMU;
+//    setOffset(getRawRotation3d());
   }
 
   /**
@@ -113,7 +114,7 @@ public class NavXSwerve extends SwerveIMU
   @Override
   public Rotation3d getRawRotation3d()
   {
-    return invertedIMU ? imu.getRotation3d().unaryMinus() : imu.getRotation3d();
+    return inverted ? imu.getRotation3d().unaryMinus() : imu.getRotation3d();
   }
 
   /**
@@ -124,7 +125,7 @@ public class NavXSwerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java
index 7d4af3a..7788334 100644
--- a/swervelib/imu/Pigeon2Swerve.java
+++ b/swervelib/imu/Pigeon2Swerve.java
@@ -152,7 +152,7 @@ public class Pigeon2Swerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
 
diff --git a/swervelib/imu/PigeonSwerve.java b/swervelib/imu/PigeonSwerve.java
index f6cfe3c..fde1d4a 100644
--- a/swervelib/imu/PigeonSwerve.java
+++ b/swervelib/imu/PigeonSwerve.java
@@ -111,7 +111,7 @@ public class PigeonSwerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/imu/PigeonViaTalonSRXSwerve.java b/swervelib/imu/PigeonViaTalonSRXSwerve.java
index 4df05fb..491b536 100644
--- a/swervelib/imu/PigeonViaTalonSRXSwerve.java
+++ b/swervelib/imu/PigeonViaTalonSRXSwerve.java
@@ -121,7 +121,7 @@ public class PigeonViaTalonSRXSwerve extends SwerveIMU
   @Override
   public Rotation3d getRotation3d()
   {
-    return getRawRotation3d().minus(offset);
+    return getRawRotation3d().rotateBy(offset.unaryMinus());
   }
 
   /**
diff --git a/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java
index b66b934..21e0bd5 100644
--- a/swervelib/motors/SparkFlexSwerve.java
+++ b/swervelib/motors/SparkFlexSwerve.java
@@ -155,7 +155,7 @@ public class SparkFlexSwerve extends SwerveMotor
   {
     if (!DriverStation.isDisabled())
     {
-      throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
+      DriverStation.reportWarning("Configuration changes cannot be applied while the robot is enabled.", false);
     }
     cfg.apply(cfgGiven);
     configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
@@ -392,10 +392,6 @@ public class SparkFlexSwerve extends SwerveMotor
   @Override
   public void burnFlash()
   {
-    if (!DriverStation.isDisabled())
-    {
-      throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
-    }
     configureSparkFlex(() -> {
       return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
     });
diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
index 9f8889b..f224dbe 100644
--- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java
+++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
@@ -214,10 +214,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
    */
   public void updateConfig(SparkMaxConfig cfgGiven)
   {
-    if (!DriverStation.isDisabled())
-    {
-      throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
-    }
     cfg.apply(cfgGiven);
     configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
   }
@@ -466,10 +462,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
   @Override
   public void burnFlash()
   {
-    if (!DriverStation.isDisabled())
-    {
-      throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
-    }
     configureSparkMax(() -> {
       return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
     });
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index 8efcea8..4acd4c9 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -148,7 +148,7 @@ public class SparkMaxSwerve extends SwerveMotor
   {
     if (!DriverStation.isDisabled())
     {
-      throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
+      DriverStation.reportWarning("Configuration changes cannot be applied while the robot is enabled.", false);
     }
     cfg.apply(cfgGiven);
     configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
@@ -391,10 +391,6 @@ public class SparkMaxSwerve extends SwerveMotor
   @Override
   public void burnFlash()
   {
-    if (!DriverStation.isDisabled())
-    {
-      throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
-    }
     configureSparkMax(() -> {
       return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
     });