Use wpilib methods lol (lmao)

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -114,7 +114,7 @@ public enum ModuleState {
     // TODO: measure these, add constants
     private double theta;
     private double r;
-    private Pose2d previousPosition;
+    private Pose2d lastPosition;
 
     public TurretSubsystem(TankSubsystem tankSubsystem, JetsonConnection connection) {
         // TODO: measure this
@@ -214,7 +214,6 @@ public void periodic() {
         } else {
             if (jetson != null) {
                 Pose2d currentPosition = tankSubsystem.getRobotPosition();
-                Pose2d deltas = tankSubsystem.poseEstimatorThread.consumeDeltas();
 
                 // If the hub is in vision range, use vision's `r` and `theta` as ground truth
                 if (jetson.turretVisionWorking()) {
@@ -231,7 +230,7 @@ public void periodic() {
                     // TODO: this assumes that the last r, theta we got is always 'clean' data.
                     // we need to do filtering of some sort, probably on the jetson, to ensure this is true.
 
-                    this.manualUpdateRTheta(deltas);
+                    this.manualUpdateRTheta(lastPosition, currentPosition);
                 }
 
                 // Set the turntable position from the relative theta given by vision
@@ -240,7 +239,7 @@ public void periodic() {
                 // TODO: constants, interpolation
                 desiredFlywheelSpeed = 30;
 
-                previousPosition = currentPosition;
+                lastPosition = currentPosition;
             }
 
             // If rejecting, scale down flywheel speed
@@ -387,7 +386,9 @@ public void setFlywheelPower(double pow) {
         flywheel.set(pow);
     }
 
-    private void manualUpdateRTheta(Pose2d deltas) {
+    private void manualUpdateRTheta(Pose2d lastPosition, Pose2d currentPosition) {
+        Pose2d deltas = lastPosition.relativeTo(currentPosition);
+
         double dx = deltas.getX();
         double dy = deltas.getY();
         double dTheta = deltas.getRotation().getRadians();

src/main/java/frc/robot/subsystems/tank/PoseEstimatorThread.java

@@ -19,27 +19,17 @@ public PoseEstimatorThread(AHRS ahrs, RelativeEncoder leftEncoder, RelativeEncod
 
         Thread thread = new Thread(runnable);
         thread.setDaemon(true);
-        //thread.start();
+        thread.start();
     }
 
     public Pose2d getPosition() {
         return runnable.getPosition();
     }
-    
+
     public void setPosition(Pose2d pose) {
         runnable.setPosition(pose);
     }
 
-    /**
-     * For turret's rtheta calculation, get the robot's relative change in position
-     * since the last call.
-     * 
-     * @return
-     */
-    public Pose2d consumeDeltas() {
-        return runnable.consumeDeltas();
-    }
-
     public DifferentialDriveWheelSpeeds getWheelSpeeds() {
         return runnable.getLastWheelSpeeds();
     }
@@ -54,24 +44,19 @@ class PoseEstimatorRunnable implements Runnable {
 
         private DifferentialDriveWheelSpeeds lastWheelSpeeds;
 
-        // for rtheta shenanigans
-        private Pose2d lastDeltaCalculationPose;
-
         public PoseEstimatorRunnable(AHRS ahrs, RelativeEncoder leftEncoder, RelativeEncoder rightEncoder) {
             this.ahrs = ahrs;
             this.leftEncoder = leftEncoder;
             this.rightEncoder = rightEncoder;
 
             // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-pose_state-estimators.html
             poseEstimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
-                // State measurement standard deviations. X, Y, theta.
-                new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
-                // Local measurement standard deviations. Left encoder, right encoder, gyro.
-                new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01),
-                // Global measurement standard deviations. X, Y, and theta.
-                new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
-
-                lastDeltaCalculationPose = poseEstimator.getEstimatedPosition();
+                    // State measurement standard deviations. X, Y, theta.
+                    new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
+                    // Local measurement standard deviations. Left encoder, right encoder, gyro.
+                    new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01),
+                    // Global measurement standard deviations. X, Y, and theta.
+                    new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
         }
 
         @Override
@@ -99,32 +84,10 @@ public void setPosition(Pose2d pose) {
             poseEstimator.resetPosition(pose, getGyroHeading());
         }
 
-        public Pose2d consumeDeltas() {
-            // i'm pretty sure this method can be done as a one-liner using methods in Pose2d
-            // but I'm not certain what what all the documentation means
-
-            Pose2d currentPose = getPosition();
-
-            double dx = currentPose.getX() - lastDeltaCalculationPose.getX();
-            double dy = currentPose.getY() - lastDeltaCalculationPose.getY();
-            double dHeading = currentPose.getRotation().getRadians() - lastDeltaCalculationPose.getRotation().getRadians();
-
-            double hypot = Math.sqrt(Math.pow(dx, 2) + Math.pow(dy, 2));
-
-            double globalheading = Math.atan2(dy, dx);
-            double relativeHeading = globalheading - currentPose.getRotation().getRadians();
-
-            double x = hypot * Math.cos(relativeHeading);
-            double y = hypot * Math.sin(relativeHeading);
-
-            // messy messy :(
-            lastDeltaCalculationPose = currentPose;
-
-            return new Pose2d(x, y, new Rotation2d(dHeading));
-        }
-
         /**
-         * Gets the gyro angle given by the NavX AHRS, inverted to be counterclockwise positive.
+         * Gets the gyro angle given by the NavX AHRS, inverted to be counterclockwise
+         * positive.
+         * 
          * @return The robot heading as a Rotation2d.
          */
         private Rotation2d getGyroHeading() {
@@ -133,17 +96,19 @@ private Rotation2d getGyroHeading() {
 
         /**
          * Gets the wheel speeds of the drivetrain.
+         * 
          * @return The drivetrain wheel speeds as a DifferentialDriveWheelSpeeds object.
          */
         private DifferentialDriveWheelSpeeds getWheelSpeeds() {
             return new DifferentialDriveWheelSpeeds(
-                leftEncoder.getVelocity(),
-                rightEncoder.getVelocity());
+                    leftEncoder.getVelocity(),
+                    rightEncoder.getVelocity());
         }
 
         /**
          * Gets the last constructed wheel speeds of the drivetrain.
-         * @return 
+         * 
+         * @return
          */
         public DifferentialDriveWheelSpeeds getLastWheelSpeeds() {
             return this.lastWheelSpeeds;